diff --git a/rclcpp/include/rclcpp/executor_options.hpp b/rclcpp/include/rclcpp/executor_options.hpp index a8fd5b73bf..56272dcbfa 100644 --- a/rclcpp/include/rclcpp/executor_options.hpp +++ b/rclcpp/include/rclcpp/executor_options.hpp @@ -43,6 +43,7 @@ struct ExecutorOptions RCLCPP_PUBLIC ExecutorOptions & operator=(const ExecutorOptions &); + [[deprecated("MemoryStrategy is deprecated.")]] rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy; rclcpp::Context::SharedPtr context; size_t max_conditions; diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index fb5ba2a63f..3b5b66c7ae 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -21,7 +21,6 @@ #include "rcl/allocator.h" #include "rcl/wait.h" - #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -39,112 +38,133 @@ namespace memory_strategy * the rmw implementation after the executor waits for work, based on the number of entities that * come through. */ -class RCLCPP_PUBLIC MemoryStrategy + +class RCLCPP_PUBLIC [[deprecated("Changes to the executor leave this Class deprecated.")]] +MemoryStrategy { public: +#if !defined(_WIN32) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +#pragma warning(push) +#pragma warning(disable : 4996) +#endif RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy) - using WeakCallbackGroupsToNodesMap = std::map>; + using WeakCallbackGroupsToNodesMap = std::map< + rclcpp::CallbackGroup::WeakPtr, rclcpp::node_interfaces::NodeBaseInterface::WeakPtr, + std::owner_less>; virtual ~MemoryStrategy() = default; + [[deprecated("MemoryStrategy is deprecated.")]] virtual bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_ready_subscriptions() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_ready_services() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_ready_clients() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_ready_events() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_ready_timers() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_guard_conditions() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual size_t number_of_waitables() const = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual void clear_handles() = 0; + [[deprecated("MemoryStrategy is deprecated.")]] virtual void remove_null_handles(rcl_wait_set_t * wait_set) = 0; - virtual void - add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void add_guard_condition(const rclcpp::GuardCondition & guard_condition) = 0; - virtual void - remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) = 0; - virtual void - get_next_subscription( + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void get_next_subscription( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; - virtual void - get_next_service( + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void get_next_service( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; - virtual void - get_next_client( + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void get_next_client( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; - virtual void - get_next_timer( + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void get_next_timer( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; - virtual void - get_next_waitable( + [[deprecated("MemoryStrategy is deprecated.")]] + virtual void get_next_waitable( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) = 0; - virtual rcl_allocator_t - get_allocator() = 0; + [[deprecated("MemoryStrategy is deprecated.")]] + virtual rcl_allocator_t get_allocator() = 0; - static rclcpp::SubscriptionBase::SharedPtr - get_subscription_by_handle( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::SubscriptionBase::SharedPtr get_subscription_by_handle( const std::shared_ptr & subscriber_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::ServiceBase::SharedPtr - get_service_by_handle( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::ServiceBase::SharedPtr get_service_by_handle( const std::shared_ptr & service_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::ClientBase::SharedPtr - get_client_by_handle( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::ClientBase::SharedPtr get_client_by_handle( const std::shared_ptr & client_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::TimerBase::SharedPtr - get_timer_by_handle( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::TimerBase::SharedPtr get_timer_by_handle( const std::shared_ptr & timer_handle, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr - get_node_by_group( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_by_group( const rclcpp::CallbackGroup::SharedPtr & group, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::CallbackGroup::SharedPtr - get_group_by_subscription( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::CallbackGroup::SharedPtr get_group_by_subscription( const rclcpp::SubscriptionBase::SharedPtr & subscription, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::CallbackGroup::SharedPtr - get_group_by_service( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::CallbackGroup::SharedPtr get_group_by_service( const rclcpp::ServiceBase::SharedPtr & service, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::CallbackGroup::SharedPtr - get_group_by_client( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::CallbackGroup::SharedPtr get_group_by_client( const rclcpp::ClientBase::SharedPtr & client, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::CallbackGroup::SharedPtr - get_group_by_timer( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::CallbackGroup::SharedPtr get_group_by_timer( const rclcpp::TimerBase::SharedPtr & timer, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); - static rclcpp::CallbackGroup::SharedPtr - get_group_by_waitable( + [[deprecated("MemoryStrategy is deprecated.")]] + static rclcpp::CallbackGroup::SharedPtr get_group_by_waitable( const rclcpp::Waitable::SharedPtr & waitable, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes); }; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index e0e2d6798b..a1d3783aba 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -19,15 +19,12 @@ #include #include "rcl/allocator.h" - #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" - #include "rcutils/logging_macros.h" - #include "rmw/types.h" namespace rclcpp @@ -43,25 +40,36 @@ namespace allocator_memory_strategy * the rmw implementation after the executor waits for work, based on the number of entities that * come through. */ -template> -class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy +template > +class [[deprecated("Changes executor leave this Class deprecated.")]] AllocatorMemoryStrategy +: public memory_strategy::MemoryStrategy { public: +#if !defined(_WIN32) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +#pragma warning(push) +#pragma warning(disable : 4996) +#endif RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy) using VoidAllocTraits = typename allocator::AllocRebind; using VoidAlloc = typename VoidAllocTraits::allocator_type; + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] explicit AllocatorMemoryStrategy(std::shared_ptr allocator) { allocator_ = std::make_shared(*allocator.get()); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] AllocatorMemoryStrategy() { allocator_ = std::make_shared(); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] void add_guard_condition(const rclcpp::GuardCondition & guard_condition) override { for (const auto & existing_guard_condition : guard_conditions_) { @@ -72,6 +80,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy guard_conditions_.push_back(&guard_condition); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] void remove_guard_condition(const rclcpp::GuardCondition * guard_condition) override { for (auto it = guard_conditions_.begin(); it != guard_conditions_.end(); ++it) { @@ -82,6 +91,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] void clear_handles() override { subscription_handles_.clear(); @@ -91,6 +101,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy waitable_handles_.clear(); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] void remove_null_handles(rcl_wait_set_t * wait_set) override { // TODO(jacobperron): Check if wait set sizes are what we expect them to be? @@ -103,9 +114,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // Mark corresponding weak_ptr as expired for entities that are null in the wait set size_t valid_subscription_count = 0; for (size_t i = 0; i < subscription_handles_.size(); ++i) { - if (valid_subscription_count < wait_set->size_of_subscriptions && - !wait_set->subscriptions[valid_subscription_count]) - { + if ( + valid_subscription_count < wait_set->size_of_subscriptions && + !wait_set->subscriptions[valid_subscription_count]) { subscription_handles_[i] = std::weak_ptr{}; } if (subscription_handles_[i].lock()) { @@ -115,9 +126,9 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t valid_service_count = 0; for (size_t i = 0; i < service_handles_.size(); ++i) { - if (valid_service_count < wait_set->size_of_services && - !wait_set->services[valid_service_count]) - { + if ( + valid_service_count < wait_set->size_of_services && + !wait_set->services[valid_service_count]) { service_handles_[i] = std::weak_ptr{}; } if (service_handles_[i].lock()) { @@ -127,9 +138,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t valid_client_count = 0; for (size_t i = 0; i < client_handles_.size(); ++i) { - if (valid_client_count < wait_set->size_of_clients && - !wait_set->clients[valid_client_count]) - { + if ( + valid_client_count < wait_set->size_of_clients && !wait_set->clients[valid_client_count]) { client_handles_[i] = std::weak_ptr{}; } if (client_handles_[i].lock()) { @@ -139,9 +149,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy size_t valid_timer_count = 0; for (size_t i = 0; i < timer_handles_.size(); ++i) { - if (valid_timer_count < wait_set->size_of_timers && - !wait_set->timers[valid_timer_count]) - { + if (valid_timer_count < wait_set->size_of_timers && !wait_set->timers[valid_timer_count]) { timer_handles_[i] = std::weak_ptr{}; } if (timer_handles_[i].lock()) { @@ -157,43 +165,37 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy // Remove expired weak_ptr instances subscription_handles_.erase( - std::remove_if(subscription_handles_.begin(), subscription_handles_.end(), - [](const std::weak_ptr & weak_ptr) { - return weak_ptr.expired(); + std::remove_if( + subscription_handles_.begin(), subscription_handles_.end(), + [](const std::weak_ptr & weak_ptr) { + return weak_ptr.expired(); }), - subscription_handles_.end() - ); + subscription_handles_.end()); service_handles_.erase( - std::remove_if(service_handles_.begin(), service_handles_.end(), - [](const std::weak_ptr & weak_ptr) { - return weak_ptr.expired(); - }), - service_handles_.end() - ); + std::remove_if( + service_handles_.begin(), service_handles_.end(), + [](const std::weak_ptr & weak_ptr) { return weak_ptr.expired(); }), + service_handles_.end()); client_handles_.erase( - std::remove_if(client_handles_.begin(), client_handles_.end(), - [](const std::weak_ptr & weak_ptr) { - return weak_ptr.expired(); - }), - client_handles_.end() - ); + std::remove_if( + client_handles_.begin(), client_handles_.end(), + [](const std::weak_ptr & weak_ptr) { return weak_ptr.expired(); }), + client_handles_.end()); timer_handles_.erase( - std::remove_if(timer_handles_.begin(), timer_handles_.end(), - [](const std::weak_ptr & weak_ptr) { - return weak_ptr.expired(); - }), - timer_handles_.end() - ); + std::remove_if( + timer_handles_.begin(), timer_handles_.end(), + [](const std::weak_ptr & weak_ptr) { return weak_ptr.expired(); }), + timer_handles_.end()); waitable_handles_.erase( std::remove(waitable_handles_.begin(), waitable_handles_.end(), nullptr), - waitable_handles_.end() - ); + waitable_handles_.end()); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] bool collect_entities(const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { bool has_invalid_weak_groups_or_nodes = false; @@ -229,6 +231,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return has_invalid_weak_groups_or_nodes; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] void add_waitable_handle(const rclcpp::Waitable::SharedPtr & waitable) override { if (nullptr == waitable) { @@ -237,19 +240,18 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy waitable_handles_.push_back(waitable); } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] bool add_handles_to_wait_set(rcl_wait_set_t * wait_set) override { for (const std::weak_ptr & weak_subscription : - subscription_handles_) - { + subscription_handles_) { auto subscription = weak_subscription.lock(); if (!subscription) { continue; // Skip expired handles } if (rcl_wait_set_add_subscription(wait_set, subscription.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add subscription to wait set: %s", rcl_get_error_string().str); rcl_reset_error(); return false; } @@ -262,8 +264,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } if (rcl_wait_set_add_client(wait_set, client.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add client to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add client to wait set: %s", rcl_get_error_string().str); rcl_reset_error(); return false; } @@ -276,8 +277,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } if (rcl_wait_set_add_service(wait_set, service.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add service to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add service to wait set: %s", rcl_get_error_string().str); rcl_reset_error(); return false; } @@ -290,8 +290,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } if (rcl_wait_set_add_timer(wait_set, timer.get(), NULL) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't add timer to wait set: %s", rcl_get_error_string().str); + "rclcpp", "Couldn't add timer to wait set: %s", rcl_get_error_string().str); rcl_reset_error(); return false; } @@ -307,8 +306,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return true; } - void - get_next_subscription( + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] + void get_next_subscription( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { @@ -348,8 +347,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void - get_next_service( + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] + void get_next_service( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { @@ -389,8 +388,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void - get_next_client( + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] + void get_next_client( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { @@ -430,8 +429,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void - get_next_timer( + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] + void get_next_timer( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { @@ -478,8 +477,8 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } - void - get_next_waitable( + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] + void get_next_waitable( rclcpp::AnyExecutable & any_exec, const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) override { @@ -513,6 +512,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] rcl_allocator_t get_allocator() override { if constexpr (std::is_same_v>) { @@ -522,6 +522,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_ready_subscriptions() const override { size_t number_of_subscriptions = 0; @@ -537,6 +538,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_subscriptions; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_ready_services() const override { size_t number_of_services = 0; @@ -552,6 +554,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_services; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_ready_events() const override { size_t number_of_events = 0; @@ -561,6 +564,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_events; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_ready_clients() const override { size_t number_of_clients = 0; @@ -576,6 +580,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_clients; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_guard_conditions() const override { size_t number_of_guard_conditions = guard_conditions_.size(); @@ -585,6 +590,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_guard_conditions; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_ready_timers() const override { size_t number_of_timers = 0; @@ -600,13 +606,14 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy return number_of_timers; } + [[deprecated("AllocatorMemoryStrategy is deprecated.")]] size_t number_of_waitables() const override { return waitable_handles_.size(); } private: - template + template using VectorRebind = std::vector::template rebind_alloc>; diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 5c6469ec69..bc7a19a618 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -36,10 +36,6 @@ if(TARGET test_exceptions) target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) endif() -ament_add_ros_isolated_gtest(test_allocator_memory_strategy strategies/test_allocator_memory_strategy.cpp) -if(TARGET test_allocator_memory_strategy) - target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME} rcpputils::rcpputils ${test_msgs_TARGETS}) -endif() ament_add_ros_isolated_gtest(test_message_pool_memory_strategy strategies/test_message_pool_memory_strategy.cpp) if(TARGET test_message_pool_memory_strategy) target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) @@ -147,9 +143,6 @@ ament_add_ros_isolated_gtest(test_loaned_message test_loaned_message.cpp) ament_add_test_label(test_loaned_message mimick) target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick ${test_msgs_TARGETS}) -ament_add_ros_isolated_gtest(test_memory_strategy test_memory_strategy.cpp) -target_link_libraries(test_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) - ament_add_ros_isolated_gtest(test_message_memory_strategy test_message_memory_strategy.cpp) target_link_libraries(test_message_memory_strategy ${PROJECT_NAME} ${test_msgs_TARGETS}) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp deleted file mode 100644 index 3748e944e6..0000000000 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ /dev/null @@ -1,912 +0,0 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" - -#include "rclcpp/strategies/allocator_memory_strategy.hpp" -#include "rcpputils/scope_exit.hpp" -#include "test_msgs/msg/empty.hpp" -#include "test_msgs/srv/empty.hpp" - -using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; -typedef std::map> WeakCallbackGroupsToNodesMap; - -static bool test_waitable_result = false; - -/** - * Mock Waitable class, with a globally setable boolean result. - */ -class TestWaitable : public rclcpp::Waitable -{ -public: - void add_to_wait_set(rcl_wait_set_t &) override - { - if (!test_waitable_result) { - throw std::runtime_error("TestWaitable add_to_wait_set failed"); - } - } - - bool is_ready(const rcl_wait_set_t &) override - { - return test_waitable_result; - } - - std::shared_ptr take_data() override {return nullptr;} - void execute(const std::shared_ptr &) override {} - - void set_on_ready_callback(std::function) override {} - void clear_on_ready_callback() override {} - - std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} - - std::vector> get_timers() const override - { - return {}; - } -}; - -struct RclWaitSetSizes -{ - size_t size_of_subscriptions = 0; - size_t size_of_guard_conditions = 0; - size_t size_of_timers = 0; - size_t size_of_clients = 0; - size_t size_of_services = 0; - size_t size_of_events = 0; - size_t size_of_waitables = 0; -}; - -// For a standard rclcpp node, this should be more than enough capacity for each type. -RclWaitSetSizes SufficientWaitSetCapacities() -{ - return {100, 100, 100, 100, 100, 100, 100}; -} - -class TestAllocatorMemoryStrategy : public ::testing::Test -{ -public: - TestAllocatorMemoryStrategy() - : allocator_memory_strategy_(nullptr) {} - - void SetUp() override - { - rclcpp::init(0, nullptr); - allocator_ = std::make_shared>(); - // Even though this is just using a basic allocator, the custom allocator constructor was - // not covered in general testing. - allocator_memory_strategy_ = std::make_shared>(allocator_); - } - - void TearDown() override - { - allocator_memory_strategy_.reset(); - rclcpp::shutdown(); - } - -protected: - std::shared_ptr> allocator_memory_strategy() - { - return allocator_memory_strategy_; - } - - // - // Convience methods, but it adds entities to vectors so the weak pointers kept by the node - // interfaces remain alive and valid - // - - std::shared_ptr create_node_with_disabled_callback_groups(const std::string & name) - { - auto node = std::make_shared(name, "ns"); - - node->for_each_callback_group( - [](rclcpp::CallbackGroup::SharedPtr group) - { - group->can_be_taken_from() = false; - }); - return node; - } - - std::shared_ptr create_node_with_subscription(const std::string & name) - { - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; - const rclcpp::QoS qos(10); - auto node_with_subscription = create_node_with_disabled_callback_groups(name); - - rclcpp::SubscriptionOptions subscription_options; - - auto callback_group = - node_with_subscription->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - subscription_options.callback_group = callback_group; - callback_groups_.push_back(callback_group); - - auto subscription = node_with_subscription->create_subscription< - test_msgs::msg::Empty, decltype(subscription_callback)>( - "topic", qos, std::move(subscription_callback), subscription_options); - subscriptions_.push_back(subscription); - - return node_with_subscription; - } - - std::shared_ptr create_node_with_service(const std::string & name) - { - auto service_callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - auto node_with_service = create_node_with_disabled_callback_groups(name); - - auto callback_group = - node_with_service->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - callback_groups_.push_back(callback_group); - - services_.push_back( - node_with_service->create_service( - "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group)); - return node_with_service; - } - - std::shared_ptr create_node_with_client(const std::string & name) - { - auto node_with_client = create_node_with_disabled_callback_groups(name); - auto callback_group = - node_with_client->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - callback_groups_.push_back(callback_group); - - clients_.push_back( - node_with_client->create_client( - "service", rclcpp::ServicesQoS(), callback_group)); - return node_with_client; - } - - std::shared_ptr create_node_with_timer(const std::string & name) - { - auto timer_callback = []() {}; - auto node_with_timer = create_node_with_disabled_callback_groups(name); - - auto callback_group = - node_with_timer->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - callback_groups_.push_back(callback_group); - - timers_.push_back( - node_with_timer->create_wall_timer( - std::chrono::milliseconds(1), timer_callback, callback_group)); - return node_with_timer; - } - - ::testing::AssertionResult TestNumberOfEntitiesAfterCollection( - std::shared_ptr node, - const RclWaitSetSizes & expected) - { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - EXPECT_EQ( - expected.size_of_subscriptions, allocator_memory_strategy()->number_of_ready_subscriptions()); - EXPECT_EQ( - expected.size_of_guard_conditions, allocator_memory_strategy()->number_of_guard_conditions()); - EXPECT_EQ(expected.size_of_timers, allocator_memory_strategy()->number_of_ready_timers()); - EXPECT_EQ(expected.size_of_clients, allocator_memory_strategy()->number_of_ready_clients()); - EXPECT_EQ(expected.size_of_services, allocator_memory_strategy()->number_of_ready_services()); - EXPECT_EQ(expected.size_of_events, allocator_memory_strategy()->number_of_ready_events()); - EXPECT_EQ(expected.size_of_waitables, allocator_memory_strategy()->number_of_waitables()); - if (::testing::Test::HasFailure()) { - return ::testing::AssertionFailure() << - "Expected number of entities did not match actual counts"; - } - return ::testing::AssertionSuccess(); - } - - ::testing::AssertionResult TestAddHandlesToWaitSet( - std::shared_ptr node, - const RclWaitSetSizes & insufficient_capacities) - { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - - auto context = node->get_node_base_interface()->get_context(); - rcl_context_t * rcl_context = context->get_rcl_context().get(); - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - RclWaitSetSizes sufficient_capacities = SufficientWaitSetCapacities(); - - rcl_ret_t ret = rcl_wait_set_init( - &wait_set, - sufficient_capacities.size_of_subscriptions, - sufficient_capacities.size_of_guard_conditions, - sufficient_capacities.size_of_timers, - sufficient_capacities.size_of_clients, - sufficient_capacities.size_of_services, - sufficient_capacities.size_of_events, - rcl_context, - allocator); - if (ret != RCL_RET_OK) { - return ::testing::AssertionFailure() << - "Calling rcl_wait_set_init() with expected sufficient capacities failed"; - } - - RCPPUTILS_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); - }); - - if (!allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)) { - return ::testing::AssertionFailure() << - "Calling add_handles_to_wait_set() with a wait_set with expected sufficient capacities" - " failed"; - } - - rcl_wait_set_t wait_set_no_capacity = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init( - &wait_set_no_capacity, - insufficient_capacities.size_of_subscriptions, - insufficient_capacities.size_of_guard_conditions, - insufficient_capacities.size_of_timers, - insufficient_capacities.size_of_clients, - insufficient_capacities.size_of_services, - insufficient_capacities.size_of_events, - rcl_context, - allocator); - - if (ret != RCL_RET_OK) { - return ::testing::AssertionFailure() << - "Calling rcl_wait_set_init() with expected insufficient capacities failed"; - } - - RCPPUTILS_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity)); - }); - - if (allocator_memory_strategy()->add_handles_to_wait_set(&wait_set_no_capacity)) { - return ::testing::AssertionFailure() << - "Calling add_handles_to_wait_set() with a wait_set with insufficient capacities" - " unexpectedly succeeded"; - } - return ::testing::AssertionSuccess(); - } - - ::testing::AssertionResult TestGetNextEntity( - std::shared_ptr node_with_entity1, - std::shared_ptr node_with_entity2, - std::function get_next_entity_func) - { - auto basic_node = create_node_with_disabled_callback_groups("basic_node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - basic_node->for_each_callback_group( - [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - basic_node->get_node_base_interface())); - }); - node_with_entity1->for_each_callback_group( - [node_with_entity1, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node_with_entity1->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - - rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes); - if (result.node_base != node_with_entity1->get_node_base_interface()) { - return ::testing::AssertionFailure() << - "Failed to get expected entity with specified get_next_*() function"; - } - - auto basic_node2 = std::make_shared("basic_node2", "ns"); - WeakCallbackGroupsToNodesMap weak_groups_to_uncollected_nodes; - basic_node2->for_each_callback_group( - [basic_node2, &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_uncollected_nodes.insert( - std::pair( - group_ptr, - basic_node2->get_node_base_interface())); - }); - node_with_entity2->for_each_callback_group( - [node_with_entity2, - &weak_groups_to_uncollected_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_uncollected_nodes.insert( - std::pair( - group_ptr, - node_with_entity2->get_node_base_interface())); - }); - rclcpp::AnyExecutable failed_result = get_next_entity_func(weak_groups_to_uncollected_nodes); - if (nullptr != failed_result.node_base) { - return ::testing::AssertionFailure() << - "A node was retrieved with the specified get_next_*() function despite" - " none of the nodes that were passed to it were added to the" - " allocator_memory_strategy. Retrieved node: " << failed_result.node_base->get_name(); - } - return ::testing::AssertionSuccess(); - } - - ::testing::AssertionResult TestGetNextEntityMutuallyExclusive( - std::shared_ptr node_with_entity, - std::function get_next_entity_func) - { - auto basic_node = std::make_shared("basic_node", "ns"); - auto basic_node_base = basic_node->get_node_base_interface(); - auto node_base = node_with_entity->get_node_base_interface(); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - basic_node_base->for_each_callback_group( - [basic_node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - basic_node_base)); - }); - node_base->for_each_callback_group( - [node_base, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node_base)); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - - // It's important that these be set after collect_entities() otherwise collect_entities() will - // not do anything - node_base->get_default_callback_group()->can_be_taken_from() = false; - basic_node_base->get_default_callback_group()->can_be_taken_from() = false; - for (auto & callback_group : callback_groups_) { - callback_group->can_be_taken_from() = false; - } - - rclcpp::AnyExecutable result = get_next_entity_func(weak_groups_to_nodes); - - if (nullptr != result.node_base) { - return ::testing::AssertionFailure() << - "A node was retrieved with the specified get_next_*() function despite" - " setting can_be_taken_from() to false for all nodes and callback_groups"; - } - - return ::testing::AssertionSuccess(); - } - - std::vector> callback_groups_; - -private: - std::shared_ptr> allocator_; - std::shared_ptr> allocator_memory_strategy_; - - // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be - // owned by this class. - std::vector subscriptions_; - std::vector services_; - std::vector clients_; - std::vector timers_; - std::vector waitables_; -}; - -TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { - auto basic_node = create_node_with_disabled_callback_groups("basic_node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - basic_node->for_each_callback_group( - [basic_node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - basic_node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_subscriptions()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_events()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); -} - -TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { - rclcpp::GuardCondition guard_condition1; - rclcpp::GuardCondition guard_condition2; - rclcpp::GuardCondition guard_condition3; - - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); - EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); - - // Adding a second time should not add to vector - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(guard_condition3)); - EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); - - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); - - // Removing second time should have no effect - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); - EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); -} - -TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { - EXPECT_THROW(allocator_memory_strategy()->add_waitable_handle(nullptr), std::runtime_error); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); - - rclcpp::Waitable::SharedPtr waitable = std::make_shared(); - EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); - EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); - - EXPECT_NO_THROW(allocator_memory_strategy()->clear_handles()); - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); -} - -TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { - RclWaitSetSizes expected_sizes = {}; - expected_sizes.size_of_subscriptions = 1; - expected_sizes.size_of_events = 2; - expected_sizes.size_of_waitables = 2; - auto node_with_subscription = create_node_with_subscription("subscription_node"); - EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); -} - -TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_service) { - RclWaitSetSizes expected_sizes = {}; - expected_sizes.size_of_services = 1; - auto node_with_service = create_node_with_service("service_node"); - EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_service, expected_sizes)); -} - -TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_client) { - RclWaitSetSizes expected_sizes = {}; - expected_sizes.size_of_clients = 1; - auto node_with_client = create_node_with_client("client_node"); - EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_client, expected_sizes)); -} - -TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) { - RclWaitSetSizes expected_sizes = {}; - expected_sizes.size_of_timers = 1; - auto node_with_timer = create_node_with_timer("timer_node"); - EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_timer, expected_sizes)); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) { - auto node = create_node_with_subscription("subscription_node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); - // The error message is collected and already reset. - EXPECT_FALSE(rcl_error_is_set()); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) { - auto node_with_subscription = create_node_with_subscription("subscription_node"); - RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); - insufficient_capacities.size_of_subscriptions = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities)); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_service) { - auto node_with_service = create_node_with_service("service_node"); - RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); - insufficient_capacities.size_of_services = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_service, insufficient_capacities)); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { - auto node_with_client = create_node_with_client("client_node"); - RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); - insufficient_capacities.size_of_clients = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_client, insufficient_capacities)); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { - auto node = create_node_with_disabled_callback_groups("node"); - auto context = node->get_node_base_interface()->get_context(); - - rclcpp::GuardCondition guard_condition(context); - - EXPECT_NO_THROW(rclcpp::GuardCondition guard_condition(context);); - - allocator_memory_strategy()->add_guard_condition(guard_condition); - - RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); - insufficient_capacities.size_of_guard_conditions = 0; - EXPECT_THROW(TestAddHandlesToWaitSet(node, insufficient_capacities), std::runtime_error); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { - auto node_with_timer = create_node_with_timer("timer_node"); - RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); - insufficient_capacities.size_of_timers = 0; - EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_timer, insufficient_capacities)); -} - -TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { - rcl_reset_error(); - - rclcpp::Waitable::SharedPtr waitable = std::make_shared(); - EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); - EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); - - test_waitable_result = true; - EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); - - test_waitable_result = false; - EXPECT_THROW( - allocator_memory_strategy()->add_handles_to_wait_set(nullptr), - std::runtime_error); - - // This calls TestWaitable's functions, so rcl errors are not set - EXPECT_FALSE(rcl_error_is_set()); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) { - auto node1 = create_node_with_subscription("node1"); - auto node2 = create_node_with_subscription("node2"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_service) { - auto node1 = create_node_with_service("node1"); - auto node2 = create_node_with_service("node2"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_client) { - auto node1 = create_node_with_client("node1"); - auto node2 = create_node_with_client("node2"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { - auto node1 = create_node_with_timer("node1"); - auto node2 = create_node_with_timer("node2"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { - auto node1 = std::make_shared("waitable_node", "ns"); - auto node2 = std::make_shared("waitable_node2", "ns"); - rclcpp::Waitable::SharedPtr waitable1 = std::make_shared(); - rclcpp::Waitable::SharedPtr waitable2 = std::make_shared(); - node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); - node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { - auto node = create_node_with_subscription("node"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) { - auto node = create_node_with_service("node"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) { - auto node = create_node_with_client("node"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) { - auto node = create_node_with_timer("node"); - - auto get_next_entity = [this](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) { - auto node = std::make_shared("waitable_node", "ns"); - rclcpp::Waitable::SharedPtr waitable = std::make_shared(); - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - node->get_node_waitables_interface()->add_waitable(waitable, callback_group); - - auto get_next_entity = - [this, callback_group](const WeakCallbackGroupsToNodesMap & weak_groups_to_nodes) { - // This callback group isn't in the base class' callback_group list, so this needs to be done - // before get_next_waitable() is called. - callback_group->can_be_taken_from() = false; - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); - return result; - }; - - EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - auto node = create_node_with_disabled_callback_groups("node"); - // Force subscription to go out of scope and cleanup after collecting entities. - { - rclcpp::SubscriptionOptions subscription_options; - - auto callback_group = - node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - subscription_options.callback_group = callback_group; - - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; - const rclcpp::QoS qos(10); - - auto subscription = node->create_subscription< - test_msgs::msg::Empty, decltype(subscription_callback)>( - "topic", qos, std::move(subscription_callback), subscription_options); - - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - } - EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_subscriptions()); - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_subscription(result, weak_groups_to_nodes); - EXPECT_EQ(nullptr, result.node_base); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - auto node = create_node_with_disabled_callback_groups("node"); - // Force service to go out of scope and cleanup after collecting entities. - { - auto callback_group = - node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - auto service_callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - auto service = node->create_service( - "service", std::move(service_callback), rclcpp::ServicesQoS(), callback_group); - - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - } - // service is out of scope here, so should be cleaned up already. - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services()); - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_service(result, weak_groups_to_nodes); - EXPECT_EQ(nullptr, result.node_base); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { - auto node = create_node_with_disabled_callback_groups("node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - // Force client to go out of scope and cleanup after collecting entities. - { - auto callback_group = - node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto client = node->create_client( - "service", rclcpp::ServicesQoS(), callback_group); - - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - node->get_node_base_interface())); - - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - } - // client is out of scope here, so should be cleaned up already. - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients()); - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_client(result, weak_groups_to_nodes); - EXPECT_EQ(nullptr, result.node_base); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) { - auto node = create_node_with_disabled_callback_groups("node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - // Force timer to go out of scope and cleanup after collecting entities. - { - auto callback_group = - node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer = node->create_wall_timer( - std::chrono::seconds(10), []() {}, callback_group); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - } - // timer is out of scope here, so should be cleaned up already. - EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers()); - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_timer(result, weak_groups_to_nodes); - EXPECT_EQ(nullptr, result.node_base); -} - -TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) { - auto node = create_node_with_disabled_callback_groups("node"); - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - // Force waitable to go out of scope and cleanup after collecting entities. - { - auto callback_group = - node->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - allocator_memory_strategy()->collect_entities(weak_groups_to_nodes); - auto waitable = std::make_shared(); - node->get_node_waitables_interface()->add_waitable(waitable, callback_group); - allocator_memory_strategy()->add_waitable_handle(waitable); - } - // Since all callback groups have been locked, except the one we added, this should only be 1 - EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); - - rclcpp::AnyExecutable result; - allocator_memory_strategy()->get_next_waitable(result, weak_groups_to_nodes); - EXPECT_EQ(nullptr, result.node_base); -} diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp deleted file mode 100644 index 4f0746e80e..0000000000 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ /dev/null @@ -1,564 +0,0 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -#include "rclcpp/strategies/allocator_memory_strategy.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "test_msgs/msg/empty.hpp" -#include "test_msgs/srv/empty.hpp" - -using rclcpp::memory_strategy::MemoryStrategy; -typedef std::map> WeakCallbackGroupsToNodesMap; - -/** - * Mock Waitable class - */ -class TestWaitable : public rclcpp::Waitable -{ -public: - void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(const rcl_wait_set_t &) override {return true;} - - std::shared_ptr take_data() override {return nullptr;} - void execute(const std::shared_ptr &) override {} - - void set_on_ready_callback(std::function) override {} - void clear_on_ready_callback() override {} - - std::shared_ptr take_data_by_entity_id(size_t) override {return nullptr;} - - std::vector> get_timers() const override - { - return {}; - } -}; - -class TestMemoryStrategy : public ::testing::Test -{ -public: - TestMemoryStrategy() - : memory_strategy_(nullptr) {} - - void SetUp() override - { - rclcpp::init(0, nullptr); - - // This doesn't test AllocatorMemoryStrategy directly, so we cast to the base class. - // AllocatorMemoryStrategy is more commonly used than MessagePoolMemoryStrategy - // so we use this derived class for these tests. - memory_strategy_ = - std::make_shared< - rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); - } - - void TearDown() override - { - rclcpp::shutdown(); - } - -protected: - std::shared_ptr memory_strategy() - { - return memory_strategy_; - } - -private: - std::shared_ptr memory_strategy_; -}; - -TEST_F(TestMemoryStrategy, construct_destruct) { - EXPECT_NE(nullptr, memory_strategy()); -} - -TEST_F(TestMemoryStrategy, get_subscription_by_handle) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - std::shared_ptr subscription_handle; - rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; - const rclcpp::QoS qos(10); - - { - auto subscription = node->create_subscription< - test_msgs::msg::Empty, decltype(subscription_callback)>( - "topic", qos, std::move(subscription_callback)); - - subscription_handle = subscription->get_subscription_handle(); - - EXPECT_EQ( - subscription, - memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); - } // subscription goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_subscription_by_handle(subscription_handle, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_service_by_handle) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - std::shared_ptr service_handle; - rclcpp::ServiceBase::SharedPtr found_service = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - const rclcpp::QoS qos(10); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - node->get_node_base_interface())); - { - auto service = node->create_service( - "service", std::move(service_callback), - rclcpp::ServicesQoS(), callback_group); - - service_handle = service->get_service_handle(); - - EXPECT_EQ( - service, - memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); - } // service goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_service_by_handle(service_handle, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_client_by_handle) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - std::shared_ptr client_handle; - rclcpp::ClientBase::SharedPtr found_client = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - { - auto client = node->create_client( - "service", rclcpp::ServicesQoS(), callback_group); - - client_handle = client->get_client_handle(); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - node->get_node_base_interface())); - - EXPECT_EQ( - client, - memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); - } // client goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - // reset client handle before node destruction, otherwise it tries to access node - // via weak_ptr in its deleter - client_handle.reset(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_client_by_handle(client_handle, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_timer_by_handle) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - std::shared_ptr timer_handle; - rclcpp::TimerBase::SharedPtr found_timer = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - { - auto timer_callback = []() {}; - auto timer = node->create_wall_timer( - std::chrono::milliseconds(1), timer_callback, callback_group); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - node->get_node_base_interface())); - - timer_handle = timer->get_timer_handle(); - - EXPECT_EQ( - timer, - memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); - } // timer goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_timer_by_handle(timer_handle, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_node_by_group) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; - { - auto node = std::make_shared("node", "ns"); - auto node_handle = node->get_node_base_interface(); - node_handle->for_each_callback_group( - [node_handle, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node_handle)); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - EXPECT_EQ( - nullptr, - memory_strategy()->get_node_by_group(nullptr, weak_groups_to_nodes)); - - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - // Nothing in the weak_groups_to_nodes, so find fails. - EXPECT_EQ( - nullptr, - memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); - - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - node->get_node_base_interface())); - EXPECT_EQ( - node_handle, - memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); - // Clear the handles to not hold NodeBase. - memory_strategy()->clear_handles(); - } // Node goes out of scope - // Callback group still exists, so lookup returns nullptr because node is destroyed. - EXPECT_EQ( - nullptr, - memory_strategy()->get_node_by_group(callback_group, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_group_by_subscription) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::SubscriptionBase::SharedPtr subscription = nullptr; - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - // This group is just used to test that a callback group that is held as a weak pointer - // by node, doesn't confuse get_group_by_subscription() when it goes out of scope - auto non_persistant_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscription_callback = [](test_msgs::msg::Empty::ConstSharedPtr) {}; - const rclcpp::QoS qos(10); - - rclcpp::SubscriptionOptions subscription_options; - - // This callback group is held as a shared_ptr in subscription_options, which means it - // stays alive as long as subscription does. - subscription_options.callback_group = callback_group; - - subscription = node->create_subscription< - test_msgs::msg::Empty, decltype(subscription_callback)>( - "topic", qos, std::move(subscription_callback), subscription_options); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - // NodeBase(SubscriptionBase->rcl_node_t->NodeBase) is still alive. - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_subscription(subscription, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_group_by_service) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::ServiceBase::SharedPtr service = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto service_callback = []( - const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - const rclcpp::QoS qos(10); - - service = node->create_service( - "service", std::move(service_callback), - rclcpp::ServicesQoS(), callback_group); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_service(service, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_group_by_client) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::ClientBase::SharedPtr client = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - client = node->create_client( - "service", rclcpp::ServicesQoS(), callback_group); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_client(client, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_group_by_timer) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::TimerBase::SharedPtr timer = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto timer_callback = []() {}; - timer = node->create_wall_timer( - std::chrono::milliseconds(1), timer_callback, callback_group); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_timer(timer, weak_groups_to_nodes)); -} - -TEST_F(TestMemoryStrategy, get_group_by_waitable) { - WeakCallbackGroupsToNodesMap weak_groups_to_nodes; - rclcpp::Waitable::SharedPtr waitable = nullptr; - { - auto node = std::make_shared("node", "ns"); - node->for_each_callback_group( - [node, &weak_groups_to_nodes](rclcpp::CallbackGroup::SharedPtr group_ptr) - { - weak_groups_to_nodes.insert( - std::pair( - group_ptr, - node->get_node_base_interface())); - }); - memory_strategy()->collect_entities(weak_groups_to_nodes); - { - waitable = std::make_shared(); - auto callback_group = - node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - node->get_node_waitables_interface()->add_waitable(waitable, callback_group); - weak_groups_to_nodes.insert( - std::pair( - rclcpp::CallbackGroup::WeakPtr(callback_group), - rclcpp::node_interfaces::NodeBaseInterface::WeakPtr(node->get_node_base_interface()))); - EXPECT_EQ( - callback_group, - memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); - } // callback_group goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); - // Clear all handles to cleanup when node is destroyed - memory_strategy()->clear_handles(); - } // Node goes out of scope - EXPECT_EQ( - nullptr, - memory_strategy()->get_group_by_waitable(waitable, weak_groups_to_nodes)); -}