@@ -120,47 +120,35 @@ TEST_F(TestMultiThreadedExecutor, starvation) {
120
120
rclcpp::TimerBase::SharedPtr timer_two;
121
121
122
122
auto timer_one_callback = [&timer_one_count, &timer_two_count]() {
123
- std::cout << " Timer one callback executed. Count: "
124
- << timer_one_count.load () << std::endl;
125
-
126
- auto start_time = std::chrono::steady_clock::now ();
127
- while (std::chrono::steady_clock::now () - start_time < 100ms) {
128
- }
123
+ std::this_thread::sleep_for (100ms);
129
124
130
125
timer_one_count++;
131
126
132
127
auto diff = std::abs (timer_one_count - timer_two_count);
133
128
134
129
std::cout << " Difference in counts: " << diff << std::endl;
135
130
136
- if (diff > 1 ) {
131
+ if (timer_one_count > 10 || timer_two_count > 10 ) {
137
132
rclcpp::shutdown ();
138
133
ASSERT_LE (diff, 1 );
139
134
}
140
135
};
141
136
142
137
auto timer_two_callback = [&timer_one_count, &timer_two_count]() {
143
- std::cout << " Timer two callback executed. Count: "
144
- << timer_two_count.load () << std::endl;
145
-
146
- auto start_time = std::chrono::steady_clock::now ();
147
- while (std::chrono::steady_clock::now () - start_time < 100ms) {
148
- }
138
+ std::this_thread::sleep_for (100ms);
149
139
150
140
timer_two_count++;
151
141
152
142
auto diff = std::abs (timer_one_count - timer_two_count);
153
143
154
- std::cout << " Difference in counts: " << diff << std::endl;
155
-
156
- if (diff > 1 ) {
144
+ if (timer_one_count > 10 || timer_two_count > 10 ) {
157
145
rclcpp::shutdown ();
158
146
ASSERT_LE (diff, 1 );
159
147
}
160
148
};
161
149
162
- timer_one = node->create_wall_timer (0ms , timer_one_callback);
163
- timer_two = node->create_wall_timer (0ms , timer_two_callback);
150
+ timer_one = node->create_wall_timer (10ms , timer_one_callback);
151
+ timer_two = node->create_wall_timer (10ms , timer_two_callback);
164
152
165
153
executor.add_node (node);
166
154
executor.spin ();
0 commit comments