diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index ca6c8f2adf..0a9ba9ca97 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -441,6 +441,24 @@ if(TARGET test_executors) target_link_libraries(test_executors ${PROJECT_NAME} rcl::rcl ${test_msgs_TARGETS}) endif() +ament_add_gtest( + test_executors_timer_cancel_behavior + executors/test_executors_timer_cancel_behavior.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_timer_cancel_behavior ${PROJECT_NAME} ${rosgraph_msgs_TARGETS}) +endif() + +ament_add_gtest( + test_executors_intraprocess + executors/test_executors_intraprocess.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) +if(TARGET test_executors) + target_link_libraries(test_executors_intraprocess ${PROJECT_NAME} ${test_msgs_TARGETS}) +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/executor_types.hpp b/rclcpp/test/rclcpp/executors/executor_types.hpp new file mode 100644 index 0000000000..0218a9b547 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/executor_types.hpp @@ -0,0 +1,70 @@ +// Copyright 2017 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__EXECUTOR_TYPES_HPP_ +#define RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ + +#include + +#include +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor, + rclcpp::experimental::executors::EventsExecutor>; + +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"; + } + + if (std::is_same()) { + return "EventsExecutor"; + } + + return ""; + } +}; + +// 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, + rclcpp::experimental::executors::EventsExecutor>; + +#endif // RCLCPP__EXECUTORS__EXECUTOR_TYPES_HPP_ diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 35eea01038..4086c14668 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -15,8 +15,8 @@ /** * 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 @@ -39,7 +39,8 @@ #include "rclcpp/time_source.hpp" #include "test_msgs/msg/empty.hpp" -#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" using namespace std::chrono_literals; @@ -86,51 +87,8 @@ class TestExecutors : public ::testing::Test template class TestExecutorsStable : public TestExecutors {}; -using ExecutorTypes = - ::testing::Types< - rclcpp::executors::SingleThreadedExecutor, - rclcpp::executors::MultiThreadedExecutor, - rclcpp::executors::StaticSingleThreadedExecutor, - rclcpp::experimental::executors::EventsExecutor>; - -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"; - } - - if (std::is_same()) { - return "EventsExecutor"; - } - - return ""; - } -}; - -// TYPED_TEST_SUITE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency -// is updated. TYPED_TEST_SUITE(TestExecutors, ExecutorTypes, ExecutorTypeNames); -// 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, - rclcpp::experimental::executors::EventsExecutor>; TYPED_TEST_SUITE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); // Make sure that executors detach from nodes when destructing @@ -719,481 +677,3 @@ TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) rclcpp::shutdown(); } - -template -class TestIntraprocessExecutors : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - 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_count = 0u; - - const std::string topic_name = std::string("topic_") + test_name.str(); - - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); - - auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { - this->callback_count.fetch_add(1u); - }; - - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - subscription = - node->create_subscription( - topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); - } - - void TearDown() - { - publisher.reset(); - subscription.reset(); - node.reset(); - } - - const size_t kNumMessages = 100; - - rclcpp::Node::SharedPtr node; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr subscription; - std::atomic_size_t callback_count; -}; - -TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { - // This tests that executors will continue to service intraprocess subscriptions in the case - // that publishers aren't continuing to publish. - // This was previously broken in that intraprocess guard conditions were only triggered on - // publish and the test was added to prevent future regressions. - static constexpr size_t kNumMessages = 100; - - using ExecutorType = TypeParam; - ExecutorType executor; - executor.add_node(this->node); - - EXPECT_EQ(0u, this->callback_count.load()); - this->publisher->publish(test_msgs::msg::Empty()); - - // Wait for up to 5 seconds for the first message to come available. - const std::chrono::milliseconds sleep_per_loop(10); - int loops = 0; - while (1u != this->callback_count.load() && loops < 500) { - rclcpp::sleep_for(sleep_per_loop); - executor.spin_some(); - loops++; - } - EXPECT_EQ(1u, this->callback_count.load()); - - // reset counter - this->callback_count.store(0u); - - for (size_t ii = 0; ii < kNumMessages; ++ii) { - this->publisher->publish(test_msgs::msg::Empty()); - } - - // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. - loops = 0; - auto timer = this->node->create_wall_timer( - std::chrono::milliseconds(10), [this, &executor, &loops]() { - loops++; - if (kNumMessages == this->callback_count.load() || loops == 500) { - executor.cancel(); - } - }); - executor.spin(); - EXPECT_EQ(kNumMessages, this->callback_count.load()); -} - -class TimerNode : public rclcpp::Node -{ -public: - explicit TimerNode(std::string subname) - : Node("timer_node", subname) - { - timer1_ = rclcpp::create_timer( - this->get_node_base_interface(), get_node_timers_interface(), - get_clock(), 1ms, - std::bind(&TimerNode::Timer1Callback, this)); - - timer2_ = - rclcpp::create_timer( - this->get_node_base_interface(), get_node_timers_interface(), - get_clock(), 1ms, - std::bind(&TimerNode::Timer2Callback, this)); - } - - int GetTimer1Cnt() {return cnt1_;} - int GetTimer2Cnt() {return cnt2_;} - - void ResetTimer1() - { - timer1_->reset(); - } - - void ResetTimer2() - { - timer2_->reset(); - } - - void CancelTimer1() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); - timer1_->cancel(); - } - - void CancelTimer2() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); - timer2_->cancel(); - } - -private: - void Timer1Callback() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); - cnt1_++; - } - - void Timer2Callback() - { - RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); - cnt2_++; - } - - rclcpp::TimerBase::SharedPtr timer1_; - rclcpp::TimerBase::SharedPtr timer2_; - int cnt1_ = 0; - int cnt2_ = 0; -}; - -// Sets up a separate thread to publish /clock messages. -// Clock rate relative to real clock is controlled by realtime_update_rate. -// This is set conservatively slow to ensure unit tests are reliable on Windows -// environments, where timing performance is subpar. -// -// Use `sleep_for` in tests to advance the clock. Clock should run and be published -// in separate thread continuously to ensure correct behavior in node under test. -class ClockPublisher : public rclcpp::Node -{ -public: - explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) - : Node("clock_publisher"), - ros_update_duration_(0, 0), - realtime_clock_step_(0, 0), - rostime_(0, 0) - { - clock_publisher_ = this->create_publisher("clock", 10); - realtime_clock_step_ = - rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); - ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); - - timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); - } - - ~ClockPublisher() - { - running_ = false; - if (timer_thread_.joinable()) { - timer_thread_.join(); - } - } - - void sleep_for(rclcpp::Duration duration) - { - rclcpp::Time start_time(0, 0, RCL_ROS_TIME); - { - const std::lock_guard lock(mutex_); - start_time = rostime_; - } - rclcpp::Time current_time = start_time; - - while (true) { - { - const std::lock_guard lock(mutex_); - current_time = rostime_; - } - if ((current_time - start_time) >= duration) { - return; - } - std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - rostime_ += ros_update_duration_; - } - } - -private: - void RunTimer() - { - while (running_) { - PublishClock(); - std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); - } - } - - void PublishClock() - { - const std::lock_guard lock(mutex_); - auto message = rosgraph_msgs::msg::Clock(); - message.clock = rostime_; - clock_publisher_->publish(message); - } - - rclcpp::Publisher::SharedPtr clock_publisher_; - - rclcpp::Duration ros_update_duration_; - rclcpp::Duration realtime_clock_step_; - // Rostime must be guarded by a mutex, since accessible in running thread - // as well as sleep_for - rclcpp::Time rostime_; - std::mutex mutex_; - std::thread timer_thread_; - std::atomic running_ = true; -}; - - -template -class TestTimerCancelBehavior : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - 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(test_name.str()); - param_client = std::make_shared(node); - ASSERT_TRUE(param_client->wait_for_service(5s)); - - auto set_parameters_results = param_client->set_parameters( - {rclcpp::Parameter("use_sim_time", false)}); - for (auto & result : set_parameters_results) { - ASSERT_TRUE(result.successful); - } - - // Run standalone thread to publish clock time - sim_clock_node = std::make_shared(); - - // Spin the executor in a standalone thread - executor.add_node(this->node); - standalone_thread = std::thread( - [this]() { - executor.spin(); - }); - } - - void TearDown() - { - node.reset(); - - // Clean up thread object - if (standalone_thread.joinable()) { - standalone_thread.join(); - } - } - - std::shared_ptr node; - std::shared_ptr sim_clock_node; - rclcpp::SyncParametersClient::SharedPtr param_client; - std::thread standalone_thread; - T executor; -}; - -TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); - -TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { - // Validate that cancelling one timer yields no change in behavior for other - // timers. Specifically, this tests the behavior when using spin() to run the - // executor, which is the most common usecase. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->sim_clock_node->sleep_for(150ms); - this->executor.cancel(); - - int t1_runs = this->node->GetTimer1Cnt(); - int t2_runs = this->node->GetTimer2Cnt(); - EXPECT_NE(t1_runs, t2_runs); - // Check that t2 has significantly more calls - EXPECT_LT(t1_runs + 50, t2_runs); -} - -TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { - // Validate that cancelling one timer yields no change in behavior for other - // timers. Specifically, this tests the behavior when using spin() to run the - // executor, which is the most common usecase. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - this->executor.cancel(); - - int t1_runs = this->node->GetTimer1Cnt(); - int t2_runs = this->node->GetTimer2Cnt(); - EXPECT_NE(t1_runs, t2_runs); - // Check that t1 has significantly more calls - EXPECT_LT(t2_runs + 50, t1_runs); -} - -TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { - // Validate that cancelling timer doesn't affect operation of other timers, - // and that the cancelled timer starts executing normally once reset manually. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 should have been restarted, and execute about 15 additional times. - // Check 10 greater than initial, to account for some timing jitter. - EXPECT_LT(t1_runs_initial + 50, t1_runs_final); - - EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); - // Check that t2 has significantly more calls, and keeps getting called. - EXPECT_LT(t2_runs_initial + 50, t2_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { - // Validate that cancelling timer doesn't affect operation of other timers, - // and that the cancelled timer starts executing normally once reset manually. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T2 should have been restarted, and execute about 15 additional times. - // Check 10 greater than initial, to account for some timing jitter. - EXPECT_LT(t2_runs_initial + 50, t2_runs_final); - - EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); - // Check that t1 has significantly more calls, and keeps getting called. - EXPECT_LT(t1_runs_initial + 50, t1_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { - // Validate behavior from cancelling 2 timers, then only re-enabling one of them. - // Ensure that only the reset timer is executed. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_intermediate = this->node->GetTimer1Cnt(); - int t2_runs_intermediate = this->node->GetTimer2Cnt(); - - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 and T2 should have the same initial count. - EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); - - // Expect that T1 has up to 15 more calls than t2. Add some buffer - // to account for jitter. - EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); - EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); - - // Expect that by end of test, both are running properly again. - EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); - EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); -} - -TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { - // Validate behavior from cancelling 2 timers, then only re-enabling one of them. - // Ensure that only the reset timer is executed. - - // Cancel to stop the spin after some time. - this->sim_clock_node->sleep_for(50ms); - this->node->CancelTimer1(); - this->node->CancelTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_initial = this->node->GetTimer1Cnt(); - int t2_runs_initial = this->node->GetTimer2Cnt(); - - // Manually reset timer 1, then sleep again - // Counts should update. - this->node->ResetTimer2(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_intermediate = this->node->GetTimer1Cnt(); - int t2_runs_intermediate = this->node->GetTimer2Cnt(); - - this->node->ResetTimer1(); - this->sim_clock_node->sleep_for(150ms); - int t1_runs_final = this->node->GetTimer1Cnt(); - int t2_runs_final = this->node->GetTimer2Cnt(); - - this->executor.cancel(); - - // T1 and T2 should have the same initial count. - EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); - - // Expect that T1 has up to 15 more calls than t2. Add some buffer - // to account for jitter. - EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); - EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); - - // Expect that by end of test, both are running properly again. - EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); - EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); -} diff --git a/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.cpp new file mode 100644 index 0000000000..af5f7e432e --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_intraprocess.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 "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" + +#include "test_msgs/msg/empty.hpp" + +#include "./executor_types.hpp" + +template +class TestIntraprocessExecutors : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + 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_count = 0u; + + const std::string topic_name = std::string("topic_") + test_name.str(); + + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + publisher = node->create_publisher(topic_name, rclcpp::QoS(1), po); + + auto callback = [this](test_msgs::msg::Empty::ConstSharedPtr) { + this->callback_count.fetch_add(1u); + }; + + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + subscription = + node->create_subscription( + topic_name, rclcpp::QoS(kNumMessages), std::move(callback), so); + } + + void TearDown() + { + publisher.reset(); + subscription.reset(); + node.reset(); + } + + const size_t kNumMessages = 100; + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + std::atomic_size_t callback_count; +}; + +TYPED_TEST_SUITE(TestIntraprocessExecutors, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestIntraprocessExecutors, testIntraprocessRetrigger) { + // This tests that executors will continue to service intraprocess subscriptions in the case + // that publishers aren't continuing to publish. + // This was previously broken in that intraprocess guard conditions were only triggered on + // publish and the test was added to prevent future regressions. + static constexpr size_t kNumMessages = 100; + + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + EXPECT_EQ(0u, this->callback_count.load()); + this->publisher->publish(test_msgs::msg::Empty()); + + // Wait for up to 5 seconds for the first message to come available. + const std::chrono::milliseconds sleep_per_loop(10); + int loops = 0; + while (1u != this->callback_count.load() && loops < 500) { + rclcpp::sleep_for(sleep_per_loop); + executor.spin_some(); + loops++; + } + EXPECT_EQ(1u, this->callback_count.load()); + + // reset counter + this->callback_count.store(0u); + + for (size_t ii = 0; ii < kNumMessages; ++ii) { + this->publisher->publish(test_msgs::msg::Empty()); + } + + // Fire a timer every 10ms up to 5 seconds waiting for subscriptions to be read. + loops = 0; + auto timer = this->node->create_wall_timer( + std::chrono::milliseconds(10), [this, &executor, &loops]() { + loops++; + if (kNumMessages == this->callback_count.load() || loops == 500) { + executor.cancel(); + } + }); + executor.spin(); + EXPECT_EQ(kNumMessages, this->callback_count.load()); +} diff --git a/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp new file mode 100644 index 0000000000..ecee459a19 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors_timer_cancel_behavior.cpp @@ -0,0 +1,408 @@ +// 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 "rclcpp/node.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/utilities.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" + +#include "./executor_types.hpp" + +using namespace std::chrono_literals; + +class TimerNode : public rclcpp::Node +{ +public: + explicit TimerNode(std::string subname) + : Node("timer_node", subname) + { + timer1_ = rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer1Callback, this)); + + timer2_ = + rclcpp::create_timer( + this->get_node_base_interface(), get_node_timers_interface(), + get_clock(), 1ms, + std::bind(&TimerNode::Timer2Callback, this)); + } + + int GetTimer1Cnt() {return cnt1_;} + int GetTimer2Cnt() {return cnt2_;} + + void ResetTimer1() + { + timer1_->reset(); + } + + void ResetTimer2() + { + timer2_->reset(); + } + + void CancelTimer1() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1 cancelling!"); + timer1_->cancel(); + } + + void CancelTimer2() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2 cancelling!"); + timer2_->cancel(); + } + +private: + void Timer1Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 1!"); + cnt1_++; + } + + void Timer2Callback() + { + RCLCPP_DEBUG(this->get_logger(), "Timer 2!"); + cnt2_++; + } + + rclcpp::TimerBase::SharedPtr timer1_; + rclcpp::TimerBase::SharedPtr timer2_; + int cnt1_ = 0; + int cnt2_ = 0; +}; + +// Sets up a separate thread to publish /clock messages. +// Clock rate relative to real clock is controlled by realtime_update_rate. +// This is set conservatively slow to ensure unit tests are reliable on Windows +// environments, where timing performance is subpar. +// +// Use `sleep_for` in tests to advance the clock. Clock should run and be published +// in separate thread continuously to ensure correct behavior in node under test. +class ClockPublisher : public rclcpp::Node +{ +public: + explicit ClockPublisher(float simulated_clock_step = .001f, float realtime_update_rate = 0.25f) + : Node("clock_publisher"), + ros_update_duration_(0, 0), + realtime_clock_step_(0, 0), + rostime_(0, 0) + { + clock_publisher_ = this->create_publisher("clock", 10); + realtime_clock_step_ = + rclcpp::Duration::from_seconds(simulated_clock_step / realtime_update_rate); + ros_update_duration_ = rclcpp::Duration::from_seconds(simulated_clock_step); + + timer_thread_ = std::thread(&ClockPublisher::RunTimer, this); + } + + ~ClockPublisher() + { + running_ = false; + if (timer_thread_.joinable()) { + timer_thread_.join(); + } + } + + void sleep_for(rclcpp::Duration duration) + { + rclcpp::Time start_time(0, 0, RCL_ROS_TIME); + { + const std::lock_guard lock(mutex_); + start_time = rostime_; + } + rclcpp::Time current_time = start_time; + + while (true) { + { + const std::lock_guard lock(mutex_); + current_time = rostime_; + } + if ((current_time - start_time) >= duration) { + return; + } + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + rostime_ += ros_update_duration_; + } + } + +private: + void RunTimer() + { + while (running_) { + PublishClock(); + std::this_thread::sleep_for(realtime_clock_step_.to_chrono()); + } + } + + void PublishClock() + { + const std::lock_guard lock(mutex_); + auto message = rosgraph_msgs::msg::Clock(); + message.clock = rostime_; + clock_publisher_->publish(message); + } + + rclcpp::Publisher::SharedPtr clock_publisher_; + + rclcpp::Duration ros_update_duration_; + rclcpp::Duration realtime_clock_step_; + // Rostime must be guarded by a mutex, since accessible in running thread + // as well as sleep_for + rclcpp::Time rostime_; + std::mutex mutex_; + std::thread timer_thread_; + std::atomic running_ = true; +}; + + +template +class TestTimerCancelBehavior : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + 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(test_name.str()); + param_client = std::make_shared(node); + ASSERT_TRUE(param_client->wait_for_service(5s)); + + auto set_parameters_results = param_client->set_parameters( + {rclcpp::Parameter("use_sim_time", false)}); + for (auto & result : set_parameters_results) { + ASSERT_TRUE(result.successful); + } + + // Run standalone thread to publish clock time + sim_clock_node = std::make_shared(); + + // Spin the executor in a standalone thread + executor.add_node(this->node); + standalone_thread = std::thread( + [this]() { + executor.spin(); + }); + } + + void TearDown() + { + node.reset(); + + // Clean up thread object + if (standalone_thread.joinable()) { + standalone_thread.join(); + } + } + + std::shared_ptr node; + std::shared_ptr sim_clock_node; + rclcpp::SyncParametersClient::SharedPtr param_client; + std::thread standalone_thread; + T executor; +}; + +TYPED_TEST_SUITE(TestTimerCancelBehavior, ExecutorTypes, ExecutorTypeNames); + +TYPED_TEST(TestTimerCancelBehavior, testTimer1CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t2 has significantly more calls + EXPECT_LT(t1_runs + 50, t2_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testTimer2CancelledWithExecutorSpin) { + // Validate that cancelling one timer yields no change in behavior for other + // timers. Specifically, this tests the behavior when using spin() to run the + // executor, which is the most common usecase. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + this->executor.cancel(); + + int t1_runs = this->node->GetTimer1Cnt(); + int t2_runs = this->node->GetTimer2Cnt(); + EXPECT_NE(t1_runs, t2_runs); + // Check that t1 has significantly more calls + EXPECT_LT(t2_runs + 50, t1_runs); +} + +TYPED_TEST(TestTimerCancelBehavior, testHeadTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); + + EXPECT_LT(t1_runs_initial + 50, t2_runs_initial); + // Check that t2 has significantly more calls, and keeps getting called. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBackTimerCancelThenResetBehavior) { + // Validate that cancelling timer doesn't affect operation of other timers, + // and that the cancelled timer starts executing normally once reset manually. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T2 should have been restarted, and execute about 15 additional times. + // Check 10 greater than initial, to account for some timing jitter. + EXPECT_LT(t2_runs_initial + 50, t2_runs_final); + + EXPECT_LT(t2_runs_initial + 50, t1_runs_initial); + // Check that t1 has significantly more calls, and keeps getting called. + EXPECT_LT(t1_runs_initial + 50, t1_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT1Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t2_runs_initial, t2_runs_intermediate); + EXPECT_LT(t1_runs_initial + 50, t1_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +} + +TYPED_TEST(TestTimerCancelBehavior, testBothTimerCancelThenResetT2Behavior) { + // Validate behavior from cancelling 2 timers, then only re-enabling one of them. + // Ensure that only the reset timer is executed. + + // Cancel to stop the spin after some time. + this->sim_clock_node->sleep_for(50ms); + this->node->CancelTimer1(); + this->node->CancelTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_initial = this->node->GetTimer1Cnt(); + int t2_runs_initial = this->node->GetTimer2Cnt(); + + // Manually reset timer 1, then sleep again + // Counts should update. + this->node->ResetTimer2(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_intermediate = this->node->GetTimer1Cnt(); + int t2_runs_intermediate = this->node->GetTimer2Cnt(); + + this->node->ResetTimer1(); + this->sim_clock_node->sleep_for(150ms); + int t1_runs_final = this->node->GetTimer1Cnt(); + int t2_runs_final = this->node->GetTimer2Cnt(); + + this->executor.cancel(); + + // T1 and T2 should have the same initial count. + EXPECT_LE(std::abs(t1_runs_initial - t2_runs_initial), 1); + + // Expect that T1 has up to 15 more calls than t2. Add some buffer + // to account for jitter. + EXPECT_EQ(t1_runs_initial, t1_runs_intermediate); + EXPECT_LT(t2_runs_initial + 50, t2_runs_intermediate); + + // Expect that by end of test, both are running properly again. + EXPECT_LT(t1_runs_intermediate + 50, t1_runs_final); + EXPECT_LT(t2_runs_intermediate + 50, t2_runs_final); +}