diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index cfd878cf32..0f5d5b9a3c 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -466,6 +466,15 @@ if(TARGET test_multi_threaded_executor) target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() +ament_add_gtest(test_static_executor_entities_collector rclcpp/executors/test_static_executor_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_static_executor_entities_collector) + ament_target_dependencies(test_static_executor_entities_collector + "rcl" + "test_msgs") + target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME}) +endif() + ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp new file mode 100644 index 0000000000..673df6ce7c --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -0,0 +1,291 @@ +// 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 "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +namespace +{ + +struct NumberOfEntities +{ + size_t subscriptions = 0; + size_t timers = 0; + size_t services = 0; + size_t clients = 0; + size_t waitables = 0; +}; + +std::unique_ptr get_number_of_default_entities(rclcpp::Node::SharedPtr node) +{ + auto number_of_entities = std::make_unique(); + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + EXPECT_NE(nullptr, group); + if (!group || !group->can_be_taken_from().load()) { + return nullptr; + } + group->find_subscription_ptrs_if( + [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &) + { + number_of_entities->subscriptions++; return false; + }); + group->find_timer_ptrs_if( + [&number_of_entities](rclcpp::TimerBase::SharedPtr &) + { + number_of_entities->timers++; return false; + }); + group->find_service_ptrs_if( + [&number_of_entities](rclcpp::ServiceBase::SharedPtr &) + { + number_of_entities->services++; return false; + }); + group->find_client_ptrs_if( + [&number_of_entities](rclcpp::ClientBase::SharedPtr &) + { + number_of_entities->clients++; return false; + }); + group->find_waitable_ptrs_if( + [&number_of_entities](rclcpp::Waitable::SharedPtr &) + { + number_of_entities->waitables++; return false; + }); + } + + return number_of_entities; +} + +} // namespace + +class TestStaticExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + entities_collector_ = + std::make_shared(); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_; +}; + +TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) { + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + EXPECT_EQ(0u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) { + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface())); + + // Check adding second time + EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface())); + + EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + // Check memory strategy is nullptr + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; + EXPECT_THROW( + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + std::runtime_error); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { + auto node = std::make_shared("node", "ns"); + const auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ( + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services()); + EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients()); + // One extra for the executor + EXPECT_EQ( + 1u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { + rclcpp::Context::SharedPtr shared_context = nullptr; + { + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + auto node3 = std::make_shared("node3", "ns"); + entities_collector_->add_node(node1->get_node_base_interface()); + entities_collector_->add_node(node2->get_node_base_interface()); + entities_collector_->add_node(node3->get_node_base_interface()); + shared_context = node1->get_node_base_interface()->get_context(); + } + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + // Expect weak_node pointers to be cleaned up and used + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + + bool is_ready(rcl_wait_set_t *) override {return true;} + + void execute() override {} +}; + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { + auto node = std::make_shared("node", "ns"); + const auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + + // Create 1 of each entity type + auto subscription = + node->create_subscription( + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + auto timer = + node->create_wall_timer(std::chrono::seconds(60), []() {}); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable, nullptr); + + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + + EXPECT_EQ( + 1u + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ( + 1u + expected_number_of_entities->services, + entities_collector_->get_number_of_services()); + EXPECT_EQ( + 1u + expected_number_of_entities->clients, + entities_collector_->get_number_of_clients()); + + // One extra for the executor + EXPECT_EQ( + 2u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + entities_collector_->remove_node(node->get_node_base_interface()); + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +}