From a5ae87c2acf6ae382cf9e49091064090a6601faa Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Thu, 9 Jul 2020 13:46:50 -0700 Subject: [PATCH 1/6] Relocate test_executor to executors directory Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 16 +- .../test_executors.cpp} | 0 ...est_static_executor_entities_collector.cpp | 285 ++++++++++++++++++ 3 files changed, 293 insertions(+), 8 deletions(-) rename rclcpp/test/rclcpp/{test_executor.cpp => executors/test_executors.cpp} (100%) create mode 100644 rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 5022024566..0fc54fb6c3 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -396,14 +396,6 @@ if(TARGET test_duration) target_link_libraries(test_duration ${PROJECT_NAME}) endif() -ament_add_gtest(test_executor rclcpp/test_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_executor) - ament_target_dependencies(test_executor - "rcl") - target_link_libraries(test_executor ${PROJECT_NAME}) -endif() - ament_add_gtest(test_logger rclcpp/test_logger.cpp) target_link_libraries(test_logger ${PROJECT_NAME}) @@ -458,6 +450,14 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_executors rclcpp/executors/test_executors.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_executors) + ament_target_dependencies(test_executors + "rcl") + target_link_libraries(test_executors ${PROJECT_NAME}) +endif() + ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_static_single_threaded_executor) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp similarity index 100% rename from rclcpp/test/rclcpp/test_executor.cpp rename to rclcpp/test/rclcpp/executors/test_executors.cpp 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..7468e9e805 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -0,0 +1,285 @@ +// 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::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + // Check memory strotegy 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)); + 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)); + + 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)); + 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()); +} From abb5b6459adf618b9db00b15984adc2a07af4c96 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Thu, 9 Jul 2020 13:42:05 -0700 Subject: [PATCH 2/6] Parametrize test_executors for all executor types Signed-off-by: Stephen Brawner --- .../test/rclcpp/executors/test_executors.cpp | 381 +++++++++++++++--- ...est_static_executor_entities_collector.cpp | 285 ------------- 2 files changed, 315 insertions(+), 351 deletions(-) delete mode 100644 rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 731add17c1..13669650a5 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -12,6 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + * + */ #include #include @@ -19,6 +24,8 @@ #include #include #include +#include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -26,94 +33,297 @@ #include "rclcpp/duration.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/empty.hpp" + using namespace std::chrono_literals; +template class TestExecutors : public ::testing::Test { -protected: - static void SetUpTestCase() +public: + void SetUp() { rclcpp::init(0, nullptr); - } + node = std::make_shared("node", "ns"); - void SetUp() - { - node = std::make_shared("my_node"); + callback_count = 0; + publisher = node->create_publisher("topic", rclcpp::QoS(10)); + auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++; }; + subscription = + node->create_subscription( + "topic", rclcpp::QoS(10), std::move(callback)); } void TearDown() { - node.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } } rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + int callback_count; }; +// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: +// https://github.com/ros2/rclcpp/issues/1219 for tracking +template +class TestExecutorsSpinVariants : public TestExecutors {}; + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; + +TYPED_TEST_CASE(TestExecutors, ExecutorTypes); + +// StaticSingleThreadedExecutor is not included in these tests for now, due to: +// https://github.com/ros2/rclcpp/issues/1219 +using StandardExecutors = +::testing::Types< +rclcpp::executors::SingleThreadedExecutor, +rclcpp::executors::MultiThreadedExecutor>; +TYPED_TEST_CASE(TestExecutorsSpinVariants, StandardExecutors); + // Make sure that executors detach from nodes when destructing -TEST_F(TestExecutors, detachOnDestruction) { +TYPED_TEST(TestExecutors, detachOnDestruction) { + using ExecutorType = TypeParam; { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); + ExecutorType executor; + executor.add_node(this->node); } { - rclcpp::executors::SingleThreadedExecutor executor; - EXPECT_NO_THROW(executor.add_node(node)); + ExecutorType executor; + EXPECT_NO_THROW(executor.add_node(this->node)); } } // Make sure that the executor can automatically remove expired nodes correctly -TEST_F(TestExecutors, addTemporaryNode) { - rclcpp::executors::SingleThreadedExecutor executor; +TYPED_TEST(TestExecutorsSpinVariants, addTemporaryNode) { + using ExecutorType = TypeParam; + ExecutorType executor; executor.add_node(std::make_shared("temporary_node")); - EXPECT_NO_THROW(executor.spin_some()); + + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + std::thread spinner([&](){EXPECT_NO_THROW(executor.spin());}); + + std::this_thread::sleep_for(50ms); + rclcpp::shutdown(); + spinner.join(); +} + +// Make sure that the executor can automatically remove expired nodes correctly +TYPED_TEST(TestExecutors, addNodeTwoExecutors) { + using ExecutorType = TypeParam; + ExecutorType executor1; + ExecutorType executor2; + EXPECT_NO_THROW(executor1.add_node(this->node)); + EXPECT_THROW(executor2.add_node(this->node), std::runtime_error); +} + +// Check simple spin example +TYPED_TEST(TestExecutors, spinWithTimer) { + using ExecutorType = TypeParam; + ExecutorType executor; + + bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&](){timer_completed=true;}); + executor.add_node(this->node); + + std::thread spinner([&](){executor.spin();}); + std::this_thread::sleep_for(50ms); + EXPECT_TRUE(timer_completed); + rclcpp::shutdown(); + spinner.join(); } -// Make sure that the spin_until_future_complete works correctly with std::future -TEST_F(TestExecutors, testSpinUntilFutureComplete) { - rclcpp::executors::SingleThreadedExecutor executor; +TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + std::thread spinner([&](){executor.spin();}); + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + std::this_thread::sleep_for(50ms); + + EXPECT_THROW(executor.spin(), std::runtime_error); + rclcpp::shutdown(); + spinner.join(); +} + +// Check executor exits immediately if future is complete. +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); std::future future; - rclcpp::FutureReturnCode ret; - // test success + // test success of an immediately finishing future future = std::async( + std::launch::async, []() { return; }); - ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - // test timeout - future = std::async( - []() { - std::this_thread::sleep_for(1s); - return; - }); - ret = executor.spin_until_future_complete(future, 100ms); - EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); + bool spin_exited = false; + + std::thread spinner([&](){ + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); + + // Force the executor to do a small amount of work + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + + EXPECT_TRUE(spin_exited); + spinner.join(); } -// Make sure that the spin_until_future_complete works correctly with std::shared_future -TEST_F(TestExecutors, testSpinUntilFutureCompleteSharedFuture) { - rclcpp::executors::SingleThreadedExecutor executor; +// Same test, but uses a shared future. +TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); std::future future; - rclcpp::FutureReturnCode ret; - // test success + // test success of an immediately finishing future future = std::async( + std::launch::async, []() { return; }); - ret = executor.spin_until_future_complete(future.share(), 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - // test timeout - future = std::async( - []() { - std::this_thread::sleep_for(1s); - return; - }); - ret = executor.spin_until_future_complete(future.share(), 100ms); - EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); + bool spin_exited = false; + + std::thread spinner([&](){ + auto shared_future = future.share(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); + + // Force the executor to do a small amount of work + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + + EXPECT_TRUE(spin_exited); + spinner.join(); +} + + + +// For a longer running future that should require several iterations of spin_once +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // This future doesn't immediately terminate, so some work gets performed. + std::futurefuture = std::async( + std::launch::async, + []() {std::this_thread::sleep_for(10ms); }); + + bool spin_exited = false; + + // Timeout set to negative for no timeout. + std::thread spinner([&](){ + auto ret = executor.spin_until_future_complete(future, -1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); + + // Do some work for longer than the future needs. + for (int i = 0; i < 100; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } + + // Not testing accuracy, just want to make sure that some work occurred. + EXPECT_LT(0, this->callback_count); + + // If this fails, the test will probably crash because spinner goes out of scope while the thread + // is active. However, it beats letting this run until the gtest timeout. + ASSERT_TRUE(spin_exited); + spinner.join(); +} + +// Check spin_until_future_complete timeout works as expected +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // Long running future relative to timeout. This timeout blocks the test finishing, so it + // shouldn't be too long. + std::futurefuture = std::async( + std::launch::async, + []() {std::this_thread::sleep_for(20ms); }); + + bool spin_exited = false; + + // Short timeout + std::thread spinner([&](){ + auto ret = executor.spin_until_future_complete(future, 10ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); + spin_exited = true; + }); + + // Do some work for longer than timeout needs. + for (int i = 0; i < 10; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } + + // Not testing accuracy, just want to make sure that some work occurred. + EXPECT_LT(0, this->callback_count); + + EXPECT_TRUE(spin_exited); + spinner.join(); +} + +// Check spin_until_future_complete can be properly interrupted. +TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // This needs to be longer than it takes to reach the shutdown call below. + // This timeout blocks the test finishing, so it shouldn't be too long. + std::futurefuture = std::async( + std::launch::async, + []() {std::this_thread::sleep_for(20ms); }); + + bool spin_exited = false; + + // Long timeout + std::thread spinner([&spin_exited, &executor, &future](){ + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); + spin_exited = true; + }); + + // Do some minimal work + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + + // Force interruption + rclcpp::shutdown(); + + // Give it time to exit + std::this_thread::sleep_for(10ms); + EXPECT_TRUE(spin_exited); + spinner.join(); } class TestWaitable : public rclcpp::Waitable @@ -156,7 +366,7 @@ class TestWaitable : public rclcpp::Waitable execute() override { count_++; - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(1ms); } size_t @@ -173,27 +383,66 @@ class TestWaitable : public rclcpp::Waitable rcl_guard_condition_t gc_; }; -TEST_F(TestExecutors, testSpinAllvsSpinSome) { - { - rclcpp::executors::SingleThreadedExecutor executor; - auto waitable_interfaces = node->get_node_waitables_interface(); - auto my_waitable = std::make_shared(); - waitable_interfaces->add_waitable(my_waitable, nullptr); - executor.add_node(node); - executor.spin_all(1s); - executor.remove_node(node); - EXPECT_GT(my_waitable->get_count(), 1u); - waitable_interfaces->remove_waitable(my_waitable, nullptr); +TYPED_TEST(TestExecutorsSpinVariants, spinAll) { + using ExecutorType = TypeParam; + ExecutorType executor; + auto waitable_interfaces = this->node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(this->node); + + // Long timeout, this determines the duration of the test, so not making it too long. + // Just long enough for multiple waitables to execute. + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this](){ + executor.spin_all(20ms); + executor.remove_node(this->node); + spin_exited = true; + }); + + // Do some work for longer the test waitable needs + for (int i = 0; i < 100; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } } - { - rclcpp::executors::SingleThreadedExecutor executor; - auto waitable_interfaces = node->get_node_waitables_interface(); - auto my_waitable = std::make_shared(); - waitable_interfaces->add_waitable(my_waitable, nullptr); - executor.add_node(node); + + EXPECT_GT(my_waitable->get_count(), 1u); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + ASSERT_TRUE(spin_exited); + spinner.join(); +} + +TYPED_TEST(TestExecutorsSpinVariants, spinSome) { + using ExecutorType = TypeParam; + ExecutorType executor; + auto waitable_interfaces = this->node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(this->node); + + // Long timeout, doesn't block test from finishing because spin_some should exit after the + // first one completes. + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this](){ executor.spin_some(1s); - executor.remove_node(node); - EXPECT_EQ(my_waitable->get_count(), 1u); - waitable_interfaces->remove_waitable(my_waitable, nullptr); + executor.remove_node(this->node); + spin_exited = true; + }); + + // Do some work for longer the test waitable needs + for (int i = 0; i < 10; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } } + + EXPECT_EQ(my_waitable->get_count(), 1u); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + ASSERT_TRUE(spin_exited); + spinner.join(); } diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp deleted file mode 100644 index 7468e9e805..0000000000 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ /dev/null @@ -1,285 +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 "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::GuardCondition guard_condition(shared_context); - rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); - - // Check memory strotegy 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)); - 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)); - - 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)); - 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()); -} From 1c33046afe403ddf88b3df29d00d4dfeba9be6ab Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Mon, 13 Jul 2020 14:08:48 -0700 Subject: [PATCH 3/6] PR Fixup Signed-off-by: Stephen Brawner --- .../test/rclcpp/executors/test_executors.cpp | 125 ++++++++++-------- 1 file changed, 67 insertions(+), 58 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 13669650a5..fd48c70aab 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -48,10 +48,10 @@ class TestExecutors : public ::testing::Test callback_count = 0; publisher = node->create_publisher("topic", rclcpp::QoS(10)); - auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++; }; + auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; subscription = node->create_subscription( - "topic", rclcpp::QoS(10), std::move(callback)); + "topic", rclcpp::QoS(10), std::move(callback)); } void TearDown() @@ -83,9 +83,9 @@ TYPED_TEST_CASE(TestExecutors, ExecutorTypes); // StaticSingleThreadedExecutor is not included in these tests for now, due to: // https://github.com/ros2/rclcpp/issues/1219 using StandardExecutors = -::testing::Types< -rclcpp::executors::SingleThreadedExecutor, -rclcpp::executors::MultiThreadedExecutor>; + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor>; TYPED_TEST_CASE(TestExecutorsSpinVariants, StandardExecutors); // Make sure that executors detach from nodes when destructing @@ -108,7 +108,7 @@ TYPED_TEST(TestExecutorsSpinVariants, addTemporaryNode) { executor.add_node(std::make_shared("temporary_node")); // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::thread spinner([&](){EXPECT_NO_THROW(executor.spin());}); + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); rclcpp::shutdown(); @@ -130,10 +130,10 @@ TYPED_TEST(TestExecutors, spinWithTimer) { ExecutorType executor; bool timer_completed = false; - auto timer = this->node->create_wall_timer(1ms, [&](){timer_completed=true;}); + auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); executor.add_node(this->node); - std::thread spinner([&](){executor.spin();}); + std::thread spinner([&]() {executor.spin();}); std::this_thread::sleep_for(50ms); EXPECT_TRUE(timer_completed); rclcpp::shutdown(); @@ -145,7 +145,7 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { ExecutorType executor; executor.add_node(this->node); - std::thread spinner([&](){executor.spin();}); + std::thread spinner([&]() {executor.spin();}); // Sleep for a short time to verify executor.spin() is going, and didn't throw. std::this_thread::sleep_for(50ms); @@ -170,15 +170,20 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { bool spin_exited = false; - std::thread spinner([&](){ - auto ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - spin_exited = true; - }); + std::thread spinner([&]() { + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); - // Force the executor to do a small amount of work - this->publisher->publish(std_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + // Do some work for longer than the future needs. + for (int i = 0; i < 100; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } EXPECT_TRUE(spin_exited); spinner.join(); @@ -200,23 +205,27 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { bool spin_exited = false; - std::thread spinner([&](){ - auto shared_future = future.share(); - auto ret = executor.spin_until_future_complete(shared_future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - spin_exited = true; - }); + std::thread spinner([&]() { + auto shared_future = future.share(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); - // Force the executor to do a small amount of work - this->publisher->publish(std_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); + // Do some work for longer than the future needs. + for (int i = 0; i < 100; ++i) { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + if (spin_exited) { + break; + } + } EXPECT_TRUE(spin_exited); spinner.join(); } - // For a longer running future that should require several iterations of spin_once TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; @@ -224,18 +233,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { executor.add_node(this->node); // This future doesn't immediately terminate, so some work gets performed. - std::futurefuture = std::async( + std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(10ms); }); + []() {std::this_thread::sleep_for(10ms);}); bool spin_exited = false; // Timeout set to negative for no timeout. - std::thread spinner([&](){ - auto ret = executor.spin_until_future_complete(future, -1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - spin_exited = true; - }); + std::thread spinner([&]() { + auto ret = executor.spin_until_future_complete(future, -1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + spin_exited = true; + }); // Do some work for longer than the future needs. for (int i = 0; i < 100; ++i) { @@ -263,18 +272,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { // Long running future relative to timeout. This timeout blocks the test finishing, so it // shouldn't be too long. - std::futurefuture = std::async( + std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(20ms); }); + []() {std::this_thread::sleep_for(20ms);}); bool spin_exited = false; // Short timeout - std::thread spinner([&](){ - auto ret = executor.spin_until_future_complete(future, 10ms); - EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); - spin_exited = true; - }); + std::thread spinner([&]() { + auto ret = executor.spin_until_future_complete(future, 10ms); + EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); + spin_exited = true; + }); // Do some work for longer than timeout needs. for (int i = 0; i < 10; ++i) { @@ -300,18 +309,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { // This needs to be longer than it takes to reach the shutdown call below. // This timeout blocks the test finishing, so it shouldn't be too long. - std::futurefuture = std::async( + std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(20ms); }); + []() {std::this_thread::sleep_for(20ms);}); bool spin_exited = false; // Long timeout - std::thread spinner([&spin_exited, &executor, &future](){ - auto ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); - spin_exited = true; - }); + std::thread spinner([&spin_exited, &executor, &future]() { + auto ret = executor.spin_until_future_complete(future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::INTERRUPTED, ret); + spin_exited = true; + }); // Do some minimal work this->publisher->publish(std_msgs::msg::Empty()); @@ -394,11 +403,11 @@ TYPED_TEST(TestExecutorsSpinVariants, spinAll) { // Long timeout, this determines the duration of the test, so not making it too long. // Just long enough for multiple waitables to execute. bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this](){ - executor.spin_all(20ms); - executor.remove_node(this->node); - spin_exited = true; - }); + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin_all(20ms); + executor.remove_node(this->node); + spin_exited = true; + }); // Do some work for longer the test waitable needs for (int i = 0; i < 100; ++i) { @@ -426,11 +435,11 @@ TYPED_TEST(TestExecutorsSpinVariants, spinSome) { // Long timeout, doesn't block test from finishing because spin_some should exit after the // first one completes. bool spin_exited = false; - std::thread spinner([&spin_exited, &executor, this](){ - executor.spin_some(1s); - executor.remove_node(this->node); - spin_exited = true; - }); + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin_some(1s); + executor.remove_node(this->node); + spin_exited = true; + }); // Do some work for longer the test waitable needs for (int i = 0; i < 10; ++i) { From e43a509fce83600f9915b6d42b6e41f1cdd562db Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Mon, 13 Jul 2020 17:50:48 -0700 Subject: [PATCH 4/6] More fixup Signed-off-by: Stephen Brawner --- .../test/rclcpp/executors/test_executors.cpp | 148 ++++++++++++------ 1 file changed, 103 insertions(+), 45 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index fd48c70aab..d6eb0620c8 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -47,11 +47,15 @@ class TestExecutors : public ::testing::Test node = std::make_shared("node", "ns"); callback_count = 0; - publisher = node->create_publisher("topic", rclcpp::QoS(10)); + std::stringstream topic_name; + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + topic_name << "topic_" << test_info->test_case_name() << "_" << test_info->name(); + + publisher = node->create_publisher(topic_name.str(), rclcpp::QoS(10)); auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; subscription = node->create_subscription( - "topic", rclcpp::QoS(10), std::move(callback)); + topic_name.str(), rclcpp::QoS(10), std::move(callback)); } void TearDown() @@ -78,7 +82,32 @@ using ExecutorTypes = rclcpp::executors::MultiThreadedExecutor, rclcpp::executors::StaticSingleThreadedExecutor>; -TYPED_TEST_CASE(TestExecutors, ExecutorTypes); +class ExecutorTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same()) { + return "SingleThreadedExecutor"; + } + + if (std::is_same()) { + return "MultiThreadedExecutor"; + } + + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + + return ""; + } +}; + +// TYPED_TEST_CASE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency +// is updated. +TYPED_TEST_CASE(TestExecutors, ExecutorTypes, ExecutorTypeNames); // StaticSingleThreadedExecutor is not included in these tests for now, due to: // https://github.com/ros2/rclcpp/issues/1219 @@ -86,7 +115,7 @@ using StandardExecutors = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor>; -TYPED_TEST_CASE(TestExecutorsSpinVariants, StandardExecutors); +TYPED_TEST_CASE(TestExecutorsSpinVariants, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { @@ -115,7 +144,7 @@ TYPED_TEST(TestExecutorsSpinVariants, addTemporaryNode) { spinner.join(); } -// Make sure that the executor can automatically remove expired nodes correctly +// Check executor throws properly if the same node is added a second time TYPED_TEST(TestExecutors, addNodeTwoExecutors) { using ExecutorType = TypeParam; ExecutorType executor1; @@ -134,8 +163,15 @@ TYPED_TEST(TestExecutors, spinWithTimer) { executor.add_node(this->node); std::thread spinner([&]() {executor.spin();}); - std::this_thread::sleep_for(50ms); + + auto start = std::chrono::steady_clock::now(); + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + EXPECT_TRUE(timer_completed); + + // Shutdown needs to be called before join, so that executor.spin() returns. rclcpp::shutdown(); spinner.join(); } @@ -145,11 +181,21 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { ExecutorType executor; executor.add_node(this->node); + bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + std::thread spinner([&]() {executor.spin();}); // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::this_thread::sleep_for(50ms); + auto start = std::chrono::steady_clock::now(); + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(timer_completed); EXPECT_THROW(executor.spin(), std::runtime_error); + + // Shutdown needs to be called before join, so that executor.spin() returns. rclcpp::shutdown(); spinner.join(); } @@ -159,14 +205,11 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); - std::future future; // test success of an immediately finishing future - future = std::async( - std::launch::async, - []() { - return; - }); + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); bool spin_exited = false; @@ -176,7 +219,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { spin_exited = true; }); - // Do some work for longer than the future needs. + // Do some work even though future is already complete to just prevent potential blocked waiting for (int i = 0; i < 100; ++i) { this->publisher->publish(std_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); @@ -194,14 +237,11 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { using ExecutorType = TypeParam; ExecutorType executor; executor.add_node(this->node); - std::future future; // test success of an immediately finishing future - future = std::async( - std::launch::async, - []() { - return; - }); + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); bool spin_exited = false; @@ -212,7 +252,7 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { spin_exited = true; }); - // Do some work for longer than the future needs. + // Do some work even though future is already complete to just prevent potential blocked waiting for (int i = 0; i < 100; ++i) { this->publisher->publish(std_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); @@ -225,7 +265,6 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { spinner.join(); } - // For a longer running future that should require several iterations of spin_once TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { using ExecutorType = TypeParam; @@ -280,13 +319,13 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { // Short timeout std::thread spinner([&]() { - auto ret = executor.spin_until_future_complete(future, 10ms); + auto ret = executor.spin_until_future_complete(future, 1ms); EXPECT_EQ(rclcpp::FutureReturnCode::TIMEOUT, ret); spin_exited = true; }); // Do some work for longer than timeout needs. - for (int i = 0; i < 10; ++i) { + for (int i = 0; i < 100; ++i) { this->publisher->publish(std_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); if (spin_exited) { @@ -307,13 +346,18 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { ExecutorType executor; executor.add_node(this->node); - // This needs to be longer than it takes to reach the shutdown call below. - // This timeout blocks the test finishing, so it shouldn't be too long. + bool spin_exited = false; + + // This needs to block longer than it takes to get to the shutdown call below and for + // spin_until_future_complete to return std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(20ms);}); - - bool spin_exited = false; + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); // Long timeout std::thread spinner([&spin_exited, &executor, &future]() { @@ -400,25 +444,33 @@ TYPED_TEST(TestExecutorsSpinVariants, spinAll) { waitable_interfaces->add_waitable(my_waitable, nullptr); executor.add_node(this->node); - // Long timeout, this determines the duration of the test, so not making it too long. - // Just long enough for multiple waitables to execute. + // Long timeout, but should not block test if spin_all works as expected as we cancel the + // executor. bool spin_exited = false; std::thread spinner([&spin_exited, &executor, this]() { - executor.spin_all(20ms); + executor.spin_all(1s); executor.remove_node(this->node); spin_exited = true; }); - // Do some work for longer the test waitable needs - for (int i = 0; i < 100; ++i) { + // Do some work until sufficient calls to the waitable occur + auto start = std::chrono::steady_clock::now(); + while ( + my_waitable->get_count() <= 1 && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { this->publisher->publish(std_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); - if (spin_exited) { - break; - } } - EXPECT_GT(my_waitable->get_count(), 1u); + executor.cancel(); + start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_LT(1u, my_waitable->get_count()); waitable_interfaces->remove_waitable(my_waitable, nullptr); ASSERT_TRUE(spin_exited); spinner.join(); @@ -441,17 +493,23 @@ TYPED_TEST(TestExecutorsSpinVariants, spinSome) { spin_exited = true; }); - // Do some work for longer the test waitable needs - for (int i = 0; i < 10; ++i) { + // Do some work until sufficient calls to the waitable occur, but keep going until either + // count becomes too large, spin exits, or the 1 second timeout completes. + auto start = std::chrono::steady_clock::now(); + while ( + my_waitable->get_count() <= 1 && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { this->publisher->publish(std_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); - if (spin_exited) { - break; - } } - EXPECT_EQ(my_waitable->get_count(), 1u); + EXPECT_EQ(1u, my_waitable->get_count()); waitable_interfaces->remove_waitable(my_waitable, nullptr); - ASSERT_TRUE(spin_exited); + EXPECT_TRUE(spin_exited); + // Cancel if it hasn't exited already. + executor.cancel(); + spinner.join(); } From ed8b51050d5b6bf9de358f5f150ab359db6aae22 Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 14 Jul 2020 15:18:11 -0700 Subject: [PATCH 5/6] Fixup Signed-off-by: Stephen Brawner --- .../test/rclcpp/executors/test_executors.cpp | 102 +++++++++--------- 1 file changed, 48 insertions(+), 54 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index d6eb0620c8..0fb8f7f57e 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -74,7 +74,7 @@ class TestExecutors : public ::testing::Test // spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: // https://github.com/ros2/rclcpp/issues/1219 for tracking template -class TestExecutorsSpinVariants : public TestExecutors {}; +class TestExecutorsStable : public TestExecutors {}; using ExecutorTypes = ::testing::Types< @@ -115,7 +115,7 @@ using StandardExecutors = ::testing::Types< rclcpp::executors::SingleThreadedExecutor, rclcpp::executors::MultiThreadedExecutor>; -TYPED_TEST_CASE(TestExecutorsSpinVariants, StandardExecutors, ExecutorTypeNames); +TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing TYPED_TEST(TestExecutors, detachOnDestruction) { @@ -131,13 +131,18 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { } // Make sure that the executor can automatically remove expired nodes correctly -TYPED_TEST(TestExecutorsSpinVariants, addTemporaryNode) { +TYPED_TEST(TestExecutorsStable, addTemporaryNode) { using ExecutorType = TypeParam; ExecutorType executor; - executor.add_node(std::make_shared("temporary_node")); + + { + // Let node go out of scope before executor.spin() + auto node = std::make_shared("temporary_node"); + executor.add_node(node); + } // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + std::thread spinner([&]() {executor.spin();}); std::this_thread::sleep_for(50ms); rclcpp::shutdown(); @@ -211,25 +216,15 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { std::future future = promise.get_future(); promise.set_value(true); - bool spin_exited = false; - - std::thread spinner([&]() { - auto ret = executor.spin_until_future_complete(future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - spin_exited = true; - }); - - // Do some work even though future is already complete to just prevent potential blocked waiting - for (int i = 0; i < 100; ++i) { - this->publisher->publish(std_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); - if (spin_exited) { - break; - } - } + // spin_until_future_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto start = std::chrono::steady_clock::now(); + auto shared_future = future.share(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); - EXPECT_TRUE(spin_exited); - spinner.join(); + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } // Same test, but uses a shared future. @@ -243,26 +238,15 @@ TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { std::future future = promise.get_future(); promise.set_value(true); - bool spin_exited = false; - - std::thread spinner([&]() { - auto shared_future = future.share(); - auto ret = executor.spin_until_future_complete(shared_future, 1s); - EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); - spin_exited = true; - }); - - // Do some work even though future is already complete to just prevent potential blocked waiting - for (int i = 0; i < 100; ++i) { - this->publisher->publish(std_msgs::msg::Empty()); - std::this_thread::sleep_for(1ms); - if (spin_exited) { - break; - } - } + // spin_until_future_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto shared_future = future.share(); + auto start = std::chrono::steady_clock::now(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); - EXPECT_TRUE(spin_exited); - spinner.join(); + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } // For a longer running future that should require several iterations of spin_once @@ -274,7 +258,12 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteNoTimeout) { // This future doesn't immediately terminate, so some work gets performed. std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(10ms);}); + [this]() { + auto start = std::chrono::steady_clock::now(); + while (this->callback_count < 1 && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); bool spin_exited = false; @@ -309,13 +298,17 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { ExecutorType executor; executor.add_node(this->node); - // Long running future relative to timeout. This timeout blocks the test finishing, so it - // shouldn't be too long. + bool spin_exited = false; + + // Needs to run longer than spin_until_future_complete's timeout. std::future future = std::async( std::launch::async, - []() {std::this_thread::sleep_for(20ms);}); - - bool spin_exited = false; + [&spin_exited]() { + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + }); // Short timeout std::thread spinner([&]() { @@ -333,9 +326,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) { } } - // Not testing accuracy, just want to make sure that some work occurred. - EXPECT_LT(0, this->callback_count); - EXPECT_TRUE(spin_exited); spinner.join(); } @@ -374,7 +364,11 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) { rclcpp::shutdown(); // Give it time to exit - std::this_thread::sleep_for(10ms); + auto start = std::chrono::steady_clock::now(); + while (!spin_exited && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + EXPECT_TRUE(spin_exited); spinner.join(); } @@ -436,7 +430,7 @@ class TestWaitable : public rclcpp::Waitable rcl_guard_condition_t gc_; }; -TYPED_TEST(TestExecutorsSpinVariants, spinAll) { +TYPED_TEST(TestExecutorsStable, spinAll) { using ExecutorType = TypeParam; ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); @@ -476,7 +470,7 @@ TYPED_TEST(TestExecutorsSpinVariants, spinAll) { spinner.join(); } -TYPED_TEST(TestExecutorsSpinVariants, spinSome) { +TYPED_TEST(TestExecutorsStable, spinSome) { using ExecutorType = TypeParam; ExecutorType executor; auto waitable_interfaces = this->node->get_node_waitables_interface(); From afb2ed0b1d148c46eb4c78e64dac8e118ccb718d Mon Sep 17 00:00:00 2001 From: Stephen Brawner Date: Tue, 14 Jul 2020 18:57:17 -0700 Subject: [PATCH 6/6] Adding issue for tracking Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/executors/test_executors.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 0fb8f7f57e..4f96122093 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -131,6 +131,8 @@ TYPED_TEST(TestExecutors, detachOnDestruction) { } // Make sure that the executor can automatically remove expired nodes correctly +// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: +// https://github.com/ros2/rclcpp/issues/1231 TYPED_TEST(TestExecutorsStable, addTemporaryNode) { using ExecutorType = TypeParam; ExecutorType executor; @@ -142,7 +144,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { } // Sleep for a short time to verify executor.spin() is going, and didn't throw. - std::thread spinner([&]() {executor.spin();}); + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); rclcpp::shutdown();