Add wait_for_all_acked support (#1662)

* Add wait_for_all_acked support

Signed-off-by: Barry Xu <[email protected]>

* Update the description of wait_for_all_acked

Signed-off-by: Barry Xu <[email protected]>

* Use rcpputils::convert_to_nanoseconds() for time conversion

Signed-off-by: Barry Xu <[email protected]>

Author: Barry-Xu-2018

rclcpp/include/rclcpp/publisher_base.hpp

@@ -18,6 +18,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <chrono>
 #include <functional>
 #include <iostream>
 #include <memory>
@@ -33,6 +34,7 @@
 #include "rclcpp/qos_event.hpp"
 #include "rclcpp/type_support_decl.hpp"
 #include "rclcpp/visibility_control.hpp"
+#include "rcpputils/time.hpp"
 
 namespace rclcpp
 {
@@ -203,6 +205,46 @@ class PublisherBase : public std::enable_shared_from_this<PublisherBase>
   std::vector<rclcpp::NetworkFlowEndpoint>
   get_network_flow_endpoints() const;
 
+  /// Wait until all published messages are acknowledged or until the specified timeout elapses.
+  /**
+   * This method waits until all published messages are acknowledged by all matching
+   * subscriptions or the given timeout elapses.
+   *
+   * If the timeout is negative then this method will block indefinitely until all published
+   * messages are acknowledged.
+   * If the timeout is zero then this method will not block, it will check if all published
+   * messages are acknowledged and return immediately.
+   * If the timeout is greater than zero, this method will wait until all published messages are
+   * acknowledged or the timeout elapses.
+   *
+   * This method only waits for acknowledgments if the publisher's QoS profile is RELIABLE.
+   * Otherwise this method will immediately return `true`.
+   *
+   * \param[in] timeout the duration to wait for all published messages to be acknowledged.
+   * \return `true` if all published messages were acknowledged before the given timeout
+   *   elapsed, otherwise `false`.
+   * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs
+   * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or
+   *   less than std::chrono::nanoseconds::min()
+   */
+  template<typename DurationRepT = int64_t, typename DurationT = std::milli>
+  bool
+  wait_for_all_acked(
+    std::chrono::duration<DurationRepT, DurationT> timeout =
+    std::chrono::duration<DurationRepT, DurationT>(-1)) const
+  {
+    rcl_duration_value_t rcl_timeout = rcpputils::convert_to_nanoseconds(timeout).count();
+
+    rcl_ret_t ret = rcl_publisher_wait_for_all_acked(publisher_handle_.get(), rcl_timeout);
+    if (ret == RCL_RET_OK) {
+      return true;
+    } else if (ret == RCL_RET_TIMEOUT) {
+      return false;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+  }
+
 protected:
   template<typename EventCallbackT>
   void

rclcpp/test/rclcpp/test_publisher.cpp

@@ -14,8 +14,9 @@
 
 #include <gtest/gtest.h>
 
-#include <string>
+#include <chrono>
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -28,6 +29,7 @@
 #include "../utils/rclcpp_gtest_macros.hpp"
 
 #include "test_msgs/msg/empty.hpp"
+#include "test_msgs/msg/strings.hpp"
 
 class TestPublisher : public ::testing::Test
 {
@@ -536,3 +538,78 @@ TEST_F(TestPublisher, get_network_flow_endpoints_errors) {
     EXPECT_NO_THROW(publisher->get_network_flow_endpoints());
   }
 }
+
+TEST_F(TestPublisher, check_wait_for_all_acked_return) {
+  initialize();
+  const rclcpp::QoS publisher_qos(1);
+  auto publisher = node->create_publisher<test_msgs::msg::Empty>("topic", publisher_qos);
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publisher_wait_for_all_acked, RCL_RET_OK);
+    EXPECT_TRUE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1)));
+  }
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publisher_wait_for_all_acked, RCL_RET_TIMEOUT);
+    EXPECT_FALSE(publisher->wait_for_all_acked(std::chrono::milliseconds(-1)));
+  }
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publisher_wait_for_all_acked, RCL_RET_UNSUPPORTED);
+    EXPECT_THROW(
+      publisher->wait_for_all_acked(std::chrono::milliseconds(-1)),
+      rclcpp::exceptions::RCLError);
+  }
+
+  {
+    // Using 'self' instead of 'lib:rclcpp' because `rcl_publisher_wait_for_all_acked` is entirely
+    // defined in a header
+    auto mock = mocking_utils::patch_and_return(
+      "self", rcl_publisher_wait_for_all_acked, RCL_RET_ERROR);
+    EXPECT_THROW(
+      publisher->wait_for_all_acked(std::chrono::milliseconds(-1)),
+      rclcpp::exceptions::RCLError);
+  }
+}
+
+class TestPublisherWaitForAllAcked
+  : public TestPublisher, public ::testing::WithParamInterface<std::pair<rclcpp::QoS, rclcpp::QoS>>
+{
+};
+
+TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) {
+  initialize();
+
+  auto do_nothing = [](std::shared_ptr<const test_msgs::msg::Strings>) {};
+  auto pub = node->create_publisher<test_msgs::msg::Strings>("topic", std::get<0>(GetParam()));
+  auto sub = node->create_subscription<test_msgs::msg::Strings>(
+    "topic",
+    std::get<1>(GetParam()),
+    do_nothing);
+
+  auto msg = std::make_shared<test_msgs::msg::Strings>();
+  for (int i = 0; i < 20; i++) {
+    ASSERT_NO_THROW(pub->publish(*msg));
+  }
+  EXPECT_TRUE(pub->wait_for_all_acked(std::chrono::milliseconds(500)));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  TestWaitForAllAckedWithParm,
+  TestPublisherWaitForAllAcked,
+  ::testing::Values(
+    std::pair<rclcpp::QoS, rclcpp::QoS>(
+      rclcpp::QoS(1).reliable(), rclcpp::QoS(1).reliable()),
+    std::pair<rclcpp::QoS, rclcpp::QoS>(
+      rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()),
+    std::pair<rclcpp::QoS, rclcpp::QoS>(
+      rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort())));