Skip to content

Commit 4baae58

Browse files
committed
Less flaky qos_event tests
Signed-off-by: Christopher Wecht <[email protected]>
1 parent 14f6e7f commit 4baae58

File tree

1 file changed

+36
-20
lines changed

1 file changed

+36
-20
lines changed

rclcpp/test/rclcpp/test_qos_event.cpp

Lines changed: 36 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -436,33 +436,38 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_set_event_callback)
436436
auto pub = node->create_publisher<test_msgs::msg::Empty>(
437437
topic_name, 10, pub_options);
438438

439-
auto matched_event_callback = [&matched_count](size_t count) {
439+
std::promise<void> prom;
440+
auto matched_event_callback = [&matched_count, &prom](size_t count) {
440441
matched_count += count;
442+
prom.set_value();
441443
};
442444

443445
pub->set_on_new_qos_event_callback(matched_event_callback, RCL_PUBLISHER_MATCHED);
444446

445447
rclcpp::executors::SingleThreadedExecutor ex;
446448
ex.add_node(node->get_node_base_interface());
447449

448-
const auto timeout = std::chrono::milliseconds(200);
450+
const auto timeout = std::chrono::seconds(10);
449451

450452
{
451453
auto sub1 = node->create_subscription<test_msgs::msg::Empty>(topic_name, 10, message_callback);
452-
ex.spin_some(timeout);
454+
ex.spin_until_future_complete(prom.get_future(), timeout);
455+
prom = {};
453456
EXPECT_EQ(matched_count, static_cast<size_t>(1));
454457

455458
{
456459
auto sub2 = node->create_subscription<test_msgs::msg::Empty>(
457460
topic_name, 10, message_callback);
458-
ex.spin_some(timeout);
461+
ex.spin_until_future_complete(prom.get_future(), timeout);
462+
prom = {};
459463
EXPECT_EQ(matched_count, static_cast<size_t>(2));
460464
}
461-
ex.spin_some(timeout);
465+
ex.spin_until_future_complete(prom.get_future(), timeout);
466+
prom = {};
462467
EXPECT_EQ(matched_count, static_cast<size_t>(3));
463468
}
464469

465-
ex.spin_some(timeout);
470+
ex.spin_until_future_complete(prom.get_future(), timeout);
466471
EXPECT_EQ(matched_count, static_cast<size_t>(4));
467472
}
468473

@@ -475,48 +480,55 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_set_event_callback)
475480
auto sub = node->create_subscription<test_msgs::msg::Empty>(
476481
topic_name, 10, message_callback, sub_options);
477482

478-
auto matched_event_callback = [&matched_count](size_t count) {
483+
std::promise<void> prom;
484+
auto matched_event_callback = [&matched_count, &prom](size_t count) {
479485
matched_count += count;
486+
prom.set_value();
480487
};
481488

482489
sub->set_on_new_qos_event_callback(matched_event_callback, RCL_SUBSCRIPTION_MATCHED);
483490

484491
rclcpp::executors::SingleThreadedExecutor ex;
485492
ex.add_node(node->get_node_base_interface());
486493

487-
const auto timeout = std::chrono::milliseconds(200);
494+
const auto timeout = std::chrono::seconds(10000);
488495

489496
{
490497
auto pub1 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
491498

492-
ex.spin_some(timeout);
499+
ex.spin_until_future_complete(prom.get_future(), timeout);
500+
prom = {};
493501
EXPECT_EQ(matched_count, static_cast<size_t>(1));
494502

495503
{
496504
auto pub2 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
497-
ex.spin_some(timeout);
505+
ex.spin_until_future_complete(prom.get_future(), timeout);
506+
prom = {};
498507
EXPECT_EQ(matched_count, static_cast<size_t>(2));
499508
}
500509

501-
ex.spin_some(timeout);
510+
ex.spin_until_future_complete(prom.get_future(), timeout);
511+
prom = {};
502512
EXPECT_EQ(matched_count, static_cast<size_t>(3));
503513
}
504514

505-
ex.spin_some(timeout);
515+
ex.spin_until_future_complete(prom.get_future(), timeout);
506516
EXPECT_EQ(matched_count, static_cast<size_t>(4));
507517
}
508518

509519
TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback)
510520
{
511521
rmw_matched_status_t matched_expected_result;
522+
std::promise<void> prom;
512523

513524
rclcpp::PublisherOptions pub_options;
514525
pub_options.event_callbacks.matched_callback =
515-
[&matched_expected_result](rmw_matched_status_t & s) {
526+
[&matched_expected_result, &prom](rmw_matched_status_t & s) {
516527
EXPECT_EQ(s.total_count, matched_expected_result.total_count);
517528
EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change);
518529
EXPECT_EQ(s.current_count, matched_expected_result.current_count);
519530
EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change);
531+
prom.set_value();
520532
};
521533

522534
auto pub = node->create_publisher<test_msgs::msg::Empty>(
@@ -531,32 +543,35 @@ TEST_F(TestQosEvent, test_pub_matched_event_by_option_event_callback)
531543
matched_expected_result.current_count = 1;
532544
matched_expected_result.current_count_change = 1;
533545

534-
const auto timeout = std::chrono::milliseconds(200);
546+
const auto timeout = std::chrono::seconds(10);
535547

536548
{
537549
auto sub = node->create_subscription<test_msgs::msg::Empty>(topic_name, 10, message_callback);
538-
ex.spin_some(timeout);
550+
ex.spin_until_future_complete(prom.get_future(), timeout);
551+
prom = {};
539552

540553
// destroy a connected subscription
541554
matched_expected_result.total_count = 1;
542555
matched_expected_result.total_count_change = 0;
543556
matched_expected_result.current_count = 0;
544557
matched_expected_result.current_count_change = -1;
545558
}
546-
ex.spin_some(timeout);
559+
ex.spin_until_future_complete(prom.get_future(), timeout);
547560
}
548561

549562
TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback)
550563
{
551564
rmw_matched_status_t matched_expected_result;
552565

566+
std::promise<void> prom;
553567
rclcpp::SubscriptionOptions sub_options;
554568
sub_options.event_callbacks.matched_callback =
555-
[&matched_expected_result](rmw_matched_status_t & s) {
569+
[&matched_expected_result, &prom](rmw_matched_status_t & s) {
556570
EXPECT_EQ(s.total_count, matched_expected_result.total_count);
557571
EXPECT_EQ(s.total_count_change, matched_expected_result.total_count_change);
558572
EXPECT_EQ(s.current_count, matched_expected_result.current_count);
559573
EXPECT_EQ(s.current_count_change, matched_expected_result.current_count_change);
574+
prom.set_value();
560575
};
561576
auto sub = node->create_subscription<test_msgs::msg::Empty>(
562577
topic_name, 10, message_callback, sub_options);
@@ -570,16 +585,17 @@ TEST_F(TestQosEvent, test_sub_matched_event_by_option_event_callback)
570585
matched_expected_result.current_count = 1;
571586
matched_expected_result.current_count_change = 1;
572587

573-
const auto timeout = std::chrono::milliseconds(200);
588+
const auto timeout = std::chrono::seconds(10);
574589
{
575590
auto pub1 = node->create_publisher<test_msgs::msg::Empty>(topic_name, 10);
576-
ex.spin_some(timeout);
591+
ex.spin_until_future_complete(prom.get_future(), timeout);
592+
prom = {};
577593

578594
// destroy a connected publisher
579595
matched_expected_result.total_count = 1;
580596
matched_expected_result.total_count_change = 0;
581597
matched_expected_result.current_count = 0;
582598
matched_expected_result.current_count_change = -1;
583599
}
584-
ex.spin_some(timeout);
600+
ex.spin_until_future_complete(prom.get_future(), timeout);
585601
}

0 commit comments

Comments
 (0)