|
29 | 29 | #include "../utils/rclcpp_gtest_macros.hpp"
|
30 | 30 |
|
31 | 31 | #include "test_msgs/msg/empty.hpp"
|
| 32 | +#include "test_msgs/msg/strings.hpp" |
32 | 33 |
|
33 | 34 | class TestPublisher : public ::testing::Test
|
34 | 35 | {
|
@@ -579,3 +580,36 @@ TEST_F(TestPublisher, check_wait_for_all_acked_return) {
|
579 | 580 | rclcpp::exceptions::RCLError);
|
580 | 581 | }
|
581 | 582 | }
|
| 583 | + |
| 584 | +class TestPublisherWaitForAllAcked |
| 585 | + : public TestPublisher, public ::testing::WithParamInterface<std::pair<rclcpp::QoS, rclcpp::QoS>> |
| 586 | +{ |
| 587 | +}; |
| 588 | + |
| 589 | +TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { |
| 590 | + initialize(); |
| 591 | + |
| 592 | + auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Strings>) {}; |
| 593 | + auto pub = node->create_publisher<test_msgs::msg::Strings>("topic", std::get<0>(GetParam())); |
| 594 | + auto sub = node->create_subscription<test_msgs::msg::Strings>( |
| 595 | + "topic", |
| 596 | + std::get<1>(GetParam()), |
| 597 | + do_nothing); |
| 598 | + |
| 599 | + auto msg = std::make_shared<test_msgs::msg::Strings>(); |
| 600 | + for (int i = 0; i < 20; i++) { |
| 601 | + ASSERT_NO_THROW(pub->publish(*msg)); |
| 602 | + } |
| 603 | + EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(500))); |
| 604 | +} |
| 605 | + |
| 606 | +INSTANTIATE_TEST_SUITE_P( |
| 607 | + TestWaitForAllAckedWithParm, |
| 608 | + TestPublisherWaitForAllAcked, |
| 609 | + ::testing::Values( |
| 610 | + std::pair<rclcpp::QoS, rclcpp::QoS>( |
| 611 | + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).reliable()), |
| 612 | + std::pair<rclcpp::QoS, rclcpp::QoS>( |
| 613 | + rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), |
| 614 | + std::pair<rclcpp::QoS, rclcpp::QoS>( |
| 615 | + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); |
0 commit comments