diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index efc30bff62..85cad29f06 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -591,6 +591,14 @@ if(TARGET test_timer) target_link_libraries(test_timer ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_reinitialized_timers test_reinitialized_timers.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_reinitialized_timers) + ament_target_dependencies(test_reinitialized_timers + "rcl") + target_link_libraries(test_reinitialized_timers ${PROJECT_NAME} mimick) +endif() + ament_add_gtest(test_timers_manager test_timers_manager.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_timers_manager) diff --git a/rclcpp/test/rclcpp/test_reinitialized_timers.cpp b/rclcpp/test/rclcpp/test_reinitialized_timers.cpp new file mode 100644 index 0000000000..4f83b329ef --- /dev/null +++ b/rclcpp/test/rclcpp/test_reinitialized_timers.cpp @@ -0,0 +1,107 @@ +// Copyright 2024 iRobot Corporation. All Rights Reserved. + +#include + +#include + +#include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp" + +using rclcpp::experimental::executors::EventsExecutor; + +class TimersTest +: public testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +TEST_F(TimersTest, TimersWithSamePeriod) +{ + auto timers_period = std::chrono::milliseconds(50); + auto node = std::make_shared("test_node"); + auto events_queue = std::make_unique(); + auto executor = std::make_unique(std::move(events_queue), true, rclcpp::ExecutorOptions()); + + executor->add_node(node); + + size_t count_1 = 0; + auto timer_1 = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(timers_period), + [&count_1]() { + count_1++; + }); + + size_t count_2 = 0; + auto timer_2 = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(timers_period), + [&count_2]() { + count_2++; + }); + + { + std::thread executor_thread([&executor](){ + executor->spin(); + }); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor->cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } + + count_1 = 0; + timer_1 = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(timers_period), + [&count_1]() { + count_1++; + }); + + count_2 = 0; + timer_2 = rclcpp::create_timer( + node, + node->get_clock(), + rclcpp::Duration(timers_period), + [&count_2]() { + count_2++; + }); + + { + std::thread executor_thread([&executor](){ + executor->spin(); + }); + + while (count_2 < 10u) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + executor->cancel(); + executor_thread.join(); + + EXPECT_GE(count_2, 10u); + EXPECT_LE(count_2 - count_1, 1u); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}