From 1855c0ccfd3b18c77b354fe9fe812b00855d370b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 28 Apr 2021 17:25:32 +0800 Subject: [PATCH 1/7] Add wait_for_all_acked support Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 64 ++++++++++++++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 45 ++++++++++++++++- 2 files changed, 108 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index c67a0439e0..6f52c74115 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -203,6 +204,69 @@ class PublisherBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + /// Wait until all published message data is acknowledged or until the specified timeout elapses. + /** + * This function waits until all published message data were acknowledged by all subscribers or + * timeout. + * + * timeout must be less then std::chrono::nanoseconds::max(). + * If the timeout is negative then this function will block indefinitely until all published + * message data were acknowledged. + * If the timeout is 0 then this function will be non-blocking; checking all published message + * data were acknowledged (If acknowledged, return true. Otherwise, return false), + * but not waiting. + * If the timeout is greater than 0 then this function will return after that period of time has + * elapsed (return false) or all published message data were acknowledged (return true). + * + * \param[in] timeout the duration to wait for all published message data were acknowledged. + * \return `true` if all published message data were acknowledged before timeout, otherwise + * `false`. + * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs + */ + template + bool + wait_for_all_acked( + std::chrono::duration timeout = + std::chrono::duration(-1)) const + { + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (timeout > ns_max_as_double) { + throw std::invalid_argument{ + "timeout must be less than std::chrono::nanoseconds::max()"}; + } + + rcl_duration_value_t rcl_timeout; + + if (timeout < std::chrono::duration::zero()) { + rcl_timeout = -1; + } else { + rcl_timeout = (std::chrono::duration_cast(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 void diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index bec8bbb17a..cc3b71a4a5 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -14,8 +14,9 @@ #include -#include +#include #include +#include #include #include @@ -536,3 +537,45 @@ 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("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); + } +} From 2e83bb5fbd51feceb292cf070ba390991a3dd7be Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 13 May 2021 19:07:26 +0800 Subject: [PATCH 2/7] Updated based on review comments Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 3 ++- rclcpp/test/rclcpp/test_publisher.cpp | 34 ++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 6f52c74115..9da311b1b5 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -209,7 +209,7 @@ class PublisherBase : public std::enable_shared_from_this * This function waits until all published message data were acknowledged by all subscribers or * timeout. * - * timeout must be less then std::chrono::nanoseconds::max(). + * timeout must be less than std::chrono::nanoseconds::max(). * If the timeout is negative then this function will block indefinitely until all published * message data were acknowledged. * If the timeout is 0 then this function will be non-blocking; checking all published message @@ -222,6 +222,7 @@ class PublisherBase : public std::enable_shared_from_this * \return `true` if all published message data were acknowledged before timeout, otherwise * `false`. * \throws rclcpp::exceptions::RCLError if middleware doesn't support or internal error occurs + * \throws std::invalid_argument if timeout is greater than nanoseconds::max() */ template bool diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index cc3b71a4a5..3be64e9647 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -29,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 { @@ -579,3 +580,36 @@ TEST_F(TestPublisher, check_wait_for_all_acked_return) { rclcpp::exceptions::RCLError); } } + +class TestPublisherWaitForAllAcked + : public TestPublisher, public ::testing::WithParamInterface> +{ +}; + +TEST_P(TestPublisherWaitForAllAcked, check_wait_for_all_acked_with_QosPolicy) { + initialize(); + + auto do_nothing = [](std::shared_ptr) {}; + auto pub = node->create_publisher("topic", std::get<0>(GetParam())); + auto sub = node->create_subscription( + "topic", + std::get<1>(GetParam()), + do_nothing); + + auto msg = std::make_shared(); + 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(1).reliable(), rclcpp::QoS(1).reliable()), + std::pair( + rclcpp::QoS(1).best_effort(), rclcpp::QoS(1).best_effort()), + std::pair( + rclcpp::QoS(1).reliable(), rclcpp::QoS(1).best_effort()))); From d383f3c6a2014a0bc60cfd48b0e03973d1c2d85a Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 23 Aug 2021 13:40:32 +0800 Subject: [PATCH 3/7] Update the description of wait_for_all_acked Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 9da311b1b5..ae3dad6bec 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -218,6 +218,9 @@ class PublisherBase : public std::enable_shared_from_this * If the timeout is greater than 0 then this function will return after that period of time has * elapsed (return false) or all published message data were acknowledged (return true). * + * This function only waits for acknowledgments if the publisher's QOS profile is RELIABLE. + * Otherwise this function will immediately return `true`. + * * \param[in] timeout the duration to wait for all published message data were acknowledged. * \return `true` if all published message data were acknowledged before timeout, otherwise * `false`. From 7ceba06114eae85bdca78636d0d297227cb31616 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 27 Sep 2021 11:28:35 +0800 Subject: [PATCH 4/7] Use rcpputils::convert_to_nanoseconds() for time conversion Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 29 ++---------------------- 1 file changed, 2 insertions(+), 27 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index ae3dad6bec..4489f604bd 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -34,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 { @@ -233,33 +234,7 @@ class PublisherBase : public std::enable_shared_from_this std::chrono::duration timeout = std::chrono::duration(-1)) const { - // Casting to a double representation might lose precision and allow the check below to succeed - // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max - constexpr auto maximum_safe_cast_ns = - std::chrono::nanoseconds::max() - std::chrono::duration(1); - - // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow - // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is - // greater than nanoseconds::max() is a difficult general problem. This is a more conservative - // version of Howard Hinnant's (the guy>) response here: - // https://stackoverflow.com/a/44637334/2089061 - // However, this doesn't solve the issue for all possible duration types of period. - // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 - constexpr auto ns_max_as_double = - std::chrono::duration_cast>( - maximum_safe_cast_ns); - if (timeout > ns_max_as_double) { - throw std::invalid_argument{ - "timeout must be less than std::chrono::nanoseconds::max()"}; - } - - rcl_duration_value_t rcl_timeout; - - if (timeout < std::chrono::duration::zero()) { - rcl_timeout = -1; - } else { - rcl_timeout = (std::chrono::duration_cast(timeout)).count(); - } + 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) { From ac0356adfc205ddac5aff934b36aa1ace9df493b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 30 Nov 2021 09:53:36 +0800 Subject: [PATCH 5/7] Address review comments Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 4489f604bd..6eeda1042d 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -207,26 +207,26 @@ class PublisherBase : public std::enable_shared_from_this /// Wait until all published message data is acknowledged or until the specified timeout elapses. /** - * This function waits until all published message data were acknowledged by all subscribers or - * timeout. + * This method waits until all published message data were acknowledged by all matching + * subscribptions or the given timeout elapses. * - * timeout must be less than std::chrono::nanoseconds::max(). - * If the timeout is negative then this function will block indefinitely until all published + * If the timeout is negative then this method will block indefinitely until all published * message data were acknowledged. - * If the timeout is 0 then this function will be non-blocking; checking all published message + * If the timeout is 0 then this method will be non-blocking; checking all published message * data were acknowledged (If acknowledged, return true. Otherwise, return false), * but not waiting. - * If the timeout is greater than 0 then this function will return after that period of time has + * If the timeout is greater than 0 then this method will return after that period of time has * elapsed (return false) or all published message data were acknowledged (return true). * - * This function only waits for acknowledgments if the publisher's QOS profile is RELIABLE. - * Otherwise this function will immediately return `true`. + * 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 message data were acknowledged. - * \return `true` if all published message data were acknowledged before timeout, otherwise - * `false`. + * \param[in] timeout the duration to wait for all published message data to be acknowledged. + * \return `true` if all published message data 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 nanoseconds::max() + * \throws std::invalid_argument if timeout is greater than std::chrono::nanoseconds::max() or + * less than std::chrono::nanoseconds::min() */ template bool From 9f927611d71b3f2cc75e454b0a977aa9001ece10 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 2 Dec 2021 10:05:13 +0800 Subject: [PATCH 6/7] Revise the description Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 6eeda1042d..e3ff00145a 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -205,18 +205,17 @@ class PublisherBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; - /// Wait until all published message data is acknowledged or until the specified timeout elapses. + /// Wait until all published messages are acknowledged or until the specified timeout elapses. /** - * This method waits until all published message data were acknowledged by all matching - * subscribptions or the given 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 - * message data were acknowledged. - * If the timeout is 0 then this method will be non-blocking; checking all published message - * data were acknowledged (If acknowledged, return true. Otherwise, return false), - * but not waiting. - * If the timeout is greater than 0 then this method will return after that period of time has - * elapsed (return false) or all published message data were acknowledged (return true). + * message data are acknowledged. + * If the timeout is zero then this method will not block, it will check if all published + * messages were 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`. From 9ac1f861a03e644acd226c73657e6f00d378df3d Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 3 Dec 2021 10:10:37 +0800 Subject: [PATCH 7/7] Revise the description Signed-off-by: Barry Xu --- rclcpp/include/rclcpp/publisher_base.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index e3ff00145a..01bdbb585f 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -211,17 +211,17 @@ class PublisherBase : public std::enable_shared_from_this * subscriptions or the given timeout elapses. * * If the timeout is negative then this method will block indefinitely until all published - * message data are acknowledged. + * messages are acknowledged. * If the timeout is zero then this method will not block, it will check if all published - * messages were acknowledged and return immediately. + * 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 message data to be acknowledged. - * \return `true` if all published message data were acknowledged before the given timeout + * \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