diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 5c607a2d6f..49e02c9489 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -462,8 +462,12 @@ endif() ament_add_gtest( test_executors executors/test_executors.cpp + executors/test_waitable.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 180) +if(CMAKE_BUILD_TYPE STREQUAL "Debug" AND MSVC) + target_compile_options(test_executors PRIVATE "/bigobj") +endif() if(TARGET test_executors) target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() @@ -495,6 +499,16 @@ if(TARGET test_executors) target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) endif() +ament_add_gtest( + test_executors_busy_waiting + executors/test_executors_busy_waiting.cpp + executors/test_waitable.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_busy_waiting ${PROJECT_NAME}) +endif() + ament_add_gtest(test_static_single_threaded_executor 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/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 789843125e..fc0eb2ff7c 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -42,6 +42,7 @@ #include "test_msgs/srv/empty.hpp" #include "./executor_types.hpp" +#include "./test_waitable.hpp" using namespace std::chrono_literals; @@ -332,114 +333,6 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteWithTimeout) spinner.join(); } -class TestWaitable : public rclcpp::Waitable -{ -public: - TestWaitable() = default; - - void - add_to_wait_set(rcl_wait_set_t & wait_set) override - { - if (trigger_count_ > 0) { - // Keep the gc triggered until the trigger count is reduced back to zero. - // This is necessary if trigger() results in the wait set waking, but not - // executing this waitable, in which case it needs to be re-triggered. - gc_.trigger(); - } - rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); - } - - void trigger() - { - trigger_count_++; - gc_.trigger(); - } - - bool - is_ready(const rcl_wait_set_t & wait_set) override - { - is_ready_count_++; - for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { - auto rcl_guard_condition = wait_set.guard_conditions[i]; - if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { - return true; - } - } - return false; - } - - std::shared_ptr - take_data() override - { - return nullptr; - } - - std::shared_ptr - take_data_by_entity_id(size_t id) override - { - (void) id; - return nullptr; - } - - void - execute(const std::shared_ptr &) override - { - trigger_count_--; - count_++; - if (nullptr != on_execute_callback_) { - on_execute_callback_(); - } else { - // TODO(wjwwood): I don't know why this was here, but probably it should - // not be there, or test cases where that is important should use the - // on_execute_callback? - std::this_thread::sleep_for(3ms); - } - } - - void - set_on_execute_callback(std::function on_execute_callback) - { - on_execute_callback_ = on_execute_callback; - } - - void - set_on_ready_callback(std::function callback) override - { - auto gc_callback = [callback](size_t count) { - callback(count, 0); - }; - gc_.set_on_trigger_callback(gc_callback); - } - - void - clear_on_ready_callback() override - { - gc_.set_on_trigger_callback(nullptr); - } - - size_t - get_number_of_ready_guard_conditions() override {return 1;} - - size_t - get_count() const - { - return count_; - } - - size_t - get_is_ready_call_count() const - { - return is_ready_count_; - } - -private: - std::atomic trigger_count_ = 0; - std::atomic is_ready_count_ = 0; - std::atomic count_ = 0; - rclcpp::GuardCondition gc_; - std::function on_execute_callback_ = nullptr; -}; - TYPED_TEST(TestExecutors, spinAll) { using ExecutorType = TypeParam; @@ -878,6 +771,7 @@ TEST(TestExecutors, testSpinWithNonDefaultContext) rclcpp::shutdown(non_default_context); } +<<<<<<< HEAD TYPED_TEST(TestExecutors, release_ownership_entity_after_spinning_cancel) { @@ -1054,3 +948,5 @@ TYPED_TEST(TestBusyWaiting, test_spin) // this should get the initial trigger, and the follow up from in the callback ASSERT_EQ(this->waitable->get_count(), 2u); } +======= +>>>>>>> c743c173 (Split test_executors.cpp even further. (#2572)) diff --git a/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp new file mode 100644 index 0000000000..bc3ce4b62d --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_busy_waiting.cpp @@ -0,0 +1,181 @@ +// Copyright 2024 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 + +#include "rclcpp/rclcpp.hpp" + +#include "./executor_types.hpp" +#include "./test_waitable.hpp" + +using namespace std::chrono_literals; + +template +class TestBusyWaiting : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); + callback_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + /* automatically_add_to_executor_with_node =*/ false); + + auto waitable_interfaces = node->get_node_waitables_interface(); + waitable = std::make_shared(); + waitable_interfaces->add_waitable(waitable, callback_group); + + executor = std::make_shared(); + executor->add_callback_group(callback_group, node->get_node_base_interface()); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + void + set_up_and_trigger_waitable(std::function extra_callback = nullptr) + { + this->has_executed = false; + this->waitable->set_on_execute_callback([this, extra_callback]() { + if (!this->has_executed) { + // trigger once to see if the second trigger is handled or not + // this follow up trigger simulates new entities becoming ready while + // the executor is executing something else, e.g. subscription got data + // or a timer expired, etc. + // spin_some would not handle this second trigger, since it collects + // work only once, whereas spin_all should handle it since it + // collects work multiple times + this->waitable->trigger(); + this->has_executed = true; + } + if (nullptr != extra_callback) { + extra_callback(); + } + }); + this->waitable->trigger(); + } + + void + check_for_busy_waits(std::chrono::steady_clock::time_point start_time) + { + // rough time based check, since the work to be done was very small it + // should be safe to check that we didn't use more than half the + // max duration, which itself is much larger than necessary + // however, it could still produce a false-positive + EXPECT_LT( + std::chrono::steady_clock::now() - start_time, + max_duration / 2) + << "executor took a long time to execute when it should have done " + << "nothing and should not have blocked either, but this could be a " + << "false negative if the computer is really slow"; + + // this check is making some assumptions about the implementation of the + // executors, but it should be safe to say that a busy wait may result in + // hundreds or thousands of calls to is_ready(), but "normal" executor + // behavior should be within an order of magnitude of the number of + // times that the waitable was executed + ASSERT_LT(waitable->get_is_ready_call_count(), 10u * this->waitable->get_count()); + } + + static constexpr auto max_duration = 10s; + + rclcpp::Node::SharedPtr node; + rclcpp::CallbackGroup::SharedPtr callback_group; + std::shared_ptr waitable; + std::chrono::steady_clock::time_point start_time; + std::shared_ptr executor; + bool has_executed; +}; + +TYPED_TEST_SUITE(TestBusyWaiting, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestBusyWaiting, test_spin_all) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_all(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} + +TYPED_TEST(TestBusyWaiting, test_spin_some) +{ + this->set_up_and_trigger_waitable(); + + auto start_time = std::chrono::steady_clock::now(); + this->executor->spin_some(this->max_duration); + this->check_for_busy_waits(start_time); + // this should get the inital trigger, but not the follow up in the callback + ASSERT_EQ(this->waitable->get_count(), 1u); +} + +TYPED_TEST(TestBusyWaiting, test_spin) +{ + std::condition_variable cv; + std::mutex cv_m; + bool first_check_passed = false; + + this->set_up_and_trigger_waitable([&cv, &cv_m, &first_check_passed]() { + cv.notify_one(); + if (!first_check_passed) { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 1s, [&]() {return first_check_passed;}); + } + }); + + auto start_time = std::chrono::steady_clock::now(); + std::thread t([this]() { + this->executor->spin(); + }); + + // wait until thread has started (first execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_GT(this->waitable->get_count(), 0u); + + first_check_passed = true; + cv.notify_one(); + + // wait until the executor has finished (second execute of waitable) + { + std::unique_lock lk(cv_m); + cv.wait_for(lk, 10s); + } + EXPECT_EQ(this->waitable->get_count(), 2u); + + this->executor->cancel(); + t.join(); + + this->check_for_busy_waits(start_time); + // this should get the initial trigger, and the follow up from in the callback + ASSERT_EQ(this->waitable->get_count(), 2u); +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.cpp b/rclcpp/test/rclcpp/executors/test_waitable.cpp new file mode 100644 index 0000000000..dfae485f30 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.cpp @@ -0,0 +1,125 @@ +// Copyright 2024 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/waitable.hpp" + +#include "rcl/wait.h" + +#include "test_waitable.hpp" + +using namespace std::chrono_literals; + +void +TestWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ + if (trigger_count_ > 0) { + // Keep the gc triggered until the trigger count is reduced back to zero. + // This is necessary if trigger() results in the wait set waking, but not + // executing this waitable, in which case it needs to be re-triggered. + gc_.trigger(); + } + rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); +} + +void TestWaitable::trigger() +{ + trigger_count_++; + gc_.trigger(); +} + +bool +TestWaitable::is_ready(const rcl_wait_set_t & wait_set) +{ + is_ready_count_++; + for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { + auto rcl_guard_condition = wait_set.guard_conditions[i]; + if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { + return true; + } + } + return false; +} + +std::shared_ptr +TestWaitable::take_data() +{ + return nullptr; +} + +std::shared_ptr +TestWaitable::take_data_by_entity_id(size_t id) +{ + (void) id; + return nullptr; +} + +void +TestWaitable::execute(const std::shared_ptr &) +{ + trigger_count_--; + count_++; + if (nullptr != on_execute_callback_) { + on_execute_callback_(); + } else { + // TODO(wjwwood): I don't know why this was here, but probably it should + // not be there, or test cases where that is important should use the + // on_execute_callback? + std::this_thread::sleep_for(3ms); + } +} + +void +TestWaitable::set_on_execute_callback(std::function on_execute_callback) +{ + on_execute_callback_ = on_execute_callback; +} + +void +TestWaitable::set_on_ready_callback(std::function callback) +{ + auto gc_callback = [callback](size_t count) { + callback(count, 0); + }; + gc_.set_on_trigger_callback(gc_callback); +} + +void +TestWaitable::clear_on_ready_callback() +{ + gc_.set_on_trigger_callback(nullptr); +} + +size_t +TestWaitable::get_number_of_ready_guard_conditions() +{ + return 1; +} + +size_t +TestWaitable::get_count() const +{ + return count_; +} + +size_t +TestWaitable::get_is_ready_call_count() const +{ + return is_ready_count_; +} diff --git a/rclcpp/test/rclcpp/executors/test_waitable.hpp b/rclcpp/test/rclcpp/executors/test_waitable.hpp new file mode 100644 index 0000000000..6c6df18578 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_waitable.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ +#define RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_ + +#include +#include +#include + +#include "rclcpp/waitable.hpp" +#include "rclcpp/guard_condition.hpp" + +#include "rcl/wait.h" + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() = default; + + void + add_to_wait_set(rcl_wait_set_t & wait_set) override; + + void trigger(); + + bool + is_ready(const rcl_wait_set_t & wait_set) override; + + std::shared_ptr + take_data() override; + + std::shared_ptr + take_data_by_entity_id(size_t id) override; + + void + execute(const std::shared_ptr &) override; + + void + set_on_execute_callback(std::function on_execute_callback); + + void + set_on_ready_callback(std::function callback) override; + + void + clear_on_ready_callback() override; + + size_t + get_number_of_ready_guard_conditions() override; + + size_t + get_count() const; + + size_t + get_is_ready_call_count() const; + +private: + std::atomic trigger_count_ = 0; + std::atomic is_ready_count_ = 0; + std::atomic count_ = 0; + rclcpp::GuardCondition gc_; + std::function on_execute_callback_ = nullptr; +}; + +#endif // RCLCPP__EXECUTORS__TEST_WAITABLE_HPP_