Skip to content

Commit

Permalink
Starvation in MultiThreadedExecutor.
Browse files Browse the repository at this point in the history
  ros2/rclcpp#2645

Signed-off-by: Tomoya Fujita <[email protected]>
  • Loading branch information
fujitatomoya committed Dec 12, 2024
1 parent cc74b85 commit ecde33e
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 0 deletions.
1 change: 1 addition & 0 deletions prover_rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,7 @@ custom_executable(rclcpp_2417_pub)
custom_executable(rclcpp_2417_sub)
custom_executable(rclcpp_2497)
custom_executable(rclcpp_2507)
custom_executable(rclcpp_2645)
#custom_executable(rclcpp_2654)
custom_executable(rclcpp_2664)

Expand Down
56 changes: 56 additions & 0 deletions prover_rclcpp/src/rclcpp_2645.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("test_timer_starvation");
auto log = node->get_logger();
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 2u);

auto cb = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

std::atomic_int timer_one_count{0};
std::atomic_int timer_two_count{0};

rclcpp::TimerBase::SharedPtr timer_one;
rclcpp::TimerBase::SharedPtr timer_two;

auto timer_one_callback = [&timer_one_count, &timer_two_count]() {
std::cout << "Timer one callback executed. Count: "
<< timer_one_count.load() << std::endl;

auto start_time = std::chrono::steady_clock::now();
while (std::chrono::steady_clock::now() - start_time < 100ms) {
}

timer_one_count++;

auto diff = std::abs(timer_one_count - timer_two_count);

std::cout << "Difference in counts: " << diff << std::endl;
};

auto timer_two_callback = [&timer_one_count, &timer_two_count]() {
std::cout << "Timer two callback executed. Count: "
<< timer_two_count.load() << std::endl;

auto start_time = std::chrono::steady_clock::now();
while (std::chrono::steady_clock::now() - start_time < 100ms) {
}

timer_two_count++;

auto diff = std::abs(timer_one_count - timer_two_count);

std::cout << "Difference in counts: " << diff << std::endl;
};

timer_one = node->create_wall_timer(50ms, timer_one_callback, cb);
timer_two = node->create_wall_timer(50ms, timer_two_callback, cb);

executor.add_node(node);
executor.spin();

rclcpp::shutdown();
}

0 comments on commit ecde33e

Please sign in to comment.