From 5a6122ac98df5c9e41c3945e6c8d19bcb108d058 Mon Sep 17 00:00:00 2001 From: Hubert Liberacki Date: Tue, 18 Jan 2022 14:36:46 +0100 Subject: [PATCH] Add support for spin_until_timeout (#1821) * Created spin_until_timeout() method * Created spin_node_until_timeout() method * Extended unit tests Signed-off-by: Hubert Liberacki --- rclcpp/include/rclcpp/executor.hpp | 49 +++++++- rclcpp/include/rclcpp/executors.hpp | 52 +++++++++ .../test/rclcpp/executors/test_executors.cpp | 105 +++++++++++++++++- 3 files changed, 201 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index f3f6e9bfe4..8304752eb1 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -344,6 +344,11 @@ class Executor } auto end_time = std::chrono::steady_clock::now(); + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_future_complete() called while already spinning"); + } + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( timeout); if (timeout_ns > std::chrono::nanoseconds::zero()) { @@ -351,9 +356,6 @@ class Executor } std::chrono::nanoseconds timeout_left = timeout_ns; - if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_future_complete() called while already spinning"); - } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. @@ -381,6 +383,47 @@ class Executor return FutureReturnCode::INTERRUPTED; } + /// Spin (blocking) until it times out, or rclcpp is interrupted. + /** + * \param[in] timeout Timeout parameter, which gets passed to Executor::spin_node_once. + * \throws std::runtime_error if spinning is already taking place. + */ + template + void spin_until_timeout( + std::chrono::duration timeout) + { + auto end_time = std::chrono::steady_clock::now(); + + if (spinning.exchange(true)) { + throw std::runtime_error("spin_until_timeout() called while already spinning"); + } + + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns <= std::chrono::nanoseconds::zero()) { + // No work to be done, timeout is reached. + return; + } + + end_time += timeout_ns; + std::chrono::nanoseconds timeout_left = timeout_ns; + + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); + while (rclcpp::ok(this->context_) && spinning.load()) { + // Do one item of work. + spin_once_impl(timeout_left); + + // Otherwise check if we still have time to wait, return TIMEOUT if not. + auto now = std::chrono::steady_clock::now(); + if (now >= end_time) { + return; + } + + // Subtract the elapsed time from the original timeout. + timeout_left = std::chrono::duration_cast(end_time - now); + } + } + /// Cancel any running spin* function, causing it to return. /** * This function can be called asynchonously from any thread. diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 36fb0d63cf..28c7f8eca9 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -54,6 +54,39 @@ namespace executors using rclcpp::executors::MultiThreadedExecutor; using rclcpp::executors::SingleThreadedExecutor; + +/// Spin (blocking) until it times out, or rclcpp is interrupted. +/** + * \param[in] executor The executor which will spin the node. + * \param[in] node_ptr The node to spin. + * \param[in] timeout Timeout parameter, which gets passed to Executor::spin_node_once. + * \throws std::runtime_error if spinning is already taking place. + */ +template +void +spin_node_until_timeout( + rclcpp::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + executor.add_node(node_ptr); + executor.spin_until_timeout(timeout); + executor.remove_node(node_ptr); +} + +template +void +spin_node_until_timeout( + rclcpp::Executor & executor, + std::shared_ptr node_ptr, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::executors::spin_node_until_timeout( + executor, + node_ptr->get_node_base_interface(), + timeout); +} + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. @@ -100,6 +133,25 @@ spin_node_until_future_complete( } // namespace executors +template +void +spin_until_timeout( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_timeout(executor, node_ptr, timeout); +} + +template +rclcpp::FutureReturnCode +spin_until_timeout( + std::shared_ptr node_ptr, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::spin_until_timeout(node_ptr->get_node_base_interface(), timeout); +} + template rclcpp::FutureReturnCode spin_until_future_complete( diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1fa2cbb4dd..f6bd9abe67 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -221,6 +221,56 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { executor.remove_node(this->node, true); } +// Check if timeout is respected +TYPED_TEST(TestExecutors, testSpinUntilTimeout) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // block intil timeout is fulfilled. + auto start = std::chrono::steady_clock::now(); + executor.spin_until_timeout(2s); + executor.remove_node(this->node, true); + // Check if timeout was fulfilled + EXPECT_GT(2s, (std::chrono::steady_clock::now() - start)); +} + +// Check if throws while spinner is already active +TYPED_TEST(TestExecutors, testSpinUntilTimeoutAlreadySpinning) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // Active spinner to check for collision. + std::thread spinner([&]() { + executor.spin_until_timeout(2s); + }); + + // There already is an active spinner running + // second call needs to throw. + EXCEPT_THROW(executor.spin_until_timeout(1ms)); + executor.remove_node(this->node, true); + executor.cancel(); + spinner.join(); +} + +// Check if timeout is respected +TYPED_TEST(TestExecutors, testSpinUntilTimeoutNegativeTimeout) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // Block intil timeout is fulfilled. + auto start = std::chrono::steady_clock::now(); + + // Spinner will exit immediately + executor.spin_until_timeout(-1s); + // Check if timeout existed without waiting + // 100ms is an arbitrary picked duration + EXPECT_LT(100ms, (std::chrono::steady_clock::now() - start)); + executor.remove_node(this->node, true); +} + // Check executor exits immediately if future is complete. TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { using ExecutorType = TypeParam; @@ -232,8 +282,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { std::future future = promise.get_future(); promise.set_value(true); - // spin_until_future_complete is expected to exit immediately, but would block up until its - // timeout if the future is not checked before spin_once_impl. + // block spin until timeout is reached auto start = std::chrono::steady_clock::now(); auto shared_future = future.share(); auto ret = executor.spin_until_future_complete(shared_future, 1s); @@ -482,6 +531,58 @@ TYPED_TEST(TestExecutors, spinSome) { spinner.join(); } +// Check spin_node_until_timeout with node base pointer +TYPED_TEST(TestExecutors, testSpinNodeUntilTimeoutNodeBasePtr) { + using ExecutorType = TypeParam; + ExecutorType executor; + + auto start = std::chrono::steady_clock::now(); + EXCEPT_NO_THROW( + rclcpp::executors::spin_node_until_timeout( + executor, this->node->get_node_base_interface(), 500ms)); + + // Make sure that timeout was reached + EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start); +} + +// Check spin_node_until_timeout with node base pointer (instantiates its own executor) +TEST(TestExecutors, testSpinNodeUntilTimeoutNodeBasePtr) { + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + auto start = std::chrono::steady_clock::now(); + + EXCEPT_NO_THROW( + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 500ms)); + + // Make sure that timeout was reached + EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start); + } + + rclcpp::shutdown(); +} + +// Check spin_until_future_complete with node pointer (instantiates its own executor) +TEST(TestExecutors, testSpinNodeUntilTimeoutNodePtr) { + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + auto start = std::chrono::steady_clock::now(); + + EXCEPT_NO_THROW( + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 500ms)); + + // Make sure that timeout was reached + EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start); + } + + rclcpp::shutdown(); +} + // Check spin_node_until_future_complete with node base pointer TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) { using ExecutorType = TypeParam;