diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index cfd7ea81fd..b9bfb52c45 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -350,32 +350,22 @@ class Executor virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. + /// Spin (blocking) until the condition is complete, it times out waiting, or rclcpp is + /// interrupted. /** - * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be - * accessed without blocking (though it may still throw an exception). + * \param[in] condition The callable condition to wait on. * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. * `-1` is block forever, `0` is non-blocking. * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template + template FutureReturnCode - spin_until_future_complete( - const FutureT & future, + spin_until_complete( + const std::function & condition, std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - auto end_time = std::chrono::steady_clock::now(); std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( timeout); @@ -384,17 +374,20 @@ class Executor } std::chrono::nanoseconds timeout_left = timeout_ns; + // Preliminary check, finish if condition is done already. + if (condition()) { + return FutureReturnCode::SUCCESS; + } + if (spinning.exchange(true)) { - throw std::runtime_error("spin_until_future_complete() called while already spinning"); + throw std::runtime_error("spin_until_complete() called while already spinning"); } RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. spin_once_impl(timeout_left); - // Check if the future is set, return SUCCESS if it is. - status = future.wait_for(std::chrono::seconds(0)); - if (status == std::future_status::ready) { + if (condition()) { return FutureReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. @@ -410,10 +403,43 @@ class Executor timeout_left = std::chrono::duration_cast(end_time - now); } - // The future did not complete before ok() returned false, return INTERRUPTED. + // The condition did not pass before ok() returned false, return INTERRUPTED. return FutureReturnCode::INTERRUPTED; } + /// Spin (blocking) for at least the given amount of duration. + /** + * \param[in] duration How long to spin for, which gets passed to Executor::spin_node_once. + */ + template + void + spin_for(std::chrono::duration timeout duration) + { + (void)spin_until_complete([]() {return false;}, duration); + } + + /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. + /** + * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be + * accessed without blocking (though it may still throw an exception). + * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return + * code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ + template + FutureReturnCode + spin_until_future_complete( + const FutureT & future, + std::chrono::duration timeout = std::chrono::duration(-1)) + { + const auto condition = [&future]() { + return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; + }; + return spin_until_complete(condition, timeout); + } + /// 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 55a38709a7..92d6903856 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -67,6 +67,48 @@ namespace executors using rclcpp::executors::MultiThreadedExecutor; using rclcpp::executors::SingleThreadedExecutor; +/// Spin (blocking) until the conditon is complete, it times out waiting, or rclcpp is interrupted. +/** + * \param[in] executor The executor which will spin the node. + * \param[in] node_ptr The node to spin. + * \param[in] condition The callable condition to wait on. + * \param[in] timeout Optional timeout parameter, which gets passed to + * Executor::spin_node_once. + * `-1` is block forever, `0` is non-blocking. + * If the time spent inside the blocking loop exceeds this timeout, return a `TIMEOUT` return code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ +template +rclcpp::FutureReturnCode +spin_node_until_complete( + rclcpp::Executor & executor, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const std::function & condition, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + // TODO(wjwwood): does not work recursively; can't call spin_node_until_complete + // inside a callback executed by an executor. + executor.add_node(node_ptr); + auto retcode = executor.spin_until_complete(condition, timeout); + executor.remove_node(node_ptr); + return retcode; +} + +template +rclcpp::FutureReturnCode +spin_node_until_complete( + rclcpp::Executor & executor, + std::shared_ptr node_ptr, + const std::function & condition, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::executors::spin_node_until_complete( + executor, + node_ptr->get_node_base_interface(), + condition, + 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. @@ -113,6 +155,27 @@ spin_node_until_future_complete( } // namespace executors +template +rclcpp::FutureReturnCode +spin_until_complete( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, + const std::function & condition, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_complete(executor, node_ptr, condition, timeout); +} + +template +rclcpp::FutureReturnCode +spin_until_complete( + std::shared_ptr node_ptr, + const std::function & condition, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), condition, timeout); +} + template rclcpp::FutureReturnCode spin_until_future_complete( diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 50af3f1a89..2c49217a24 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -69,6 +69,7 @@ * - Executors (responsible for execution of callbacks through a blocking spin): * - rclcpp::spin() * - rclcpp::spin_some() + * - rclcpp::spin_until_complete() * - rclcpp::spin_until_future_complete() * - rclcpp::executors::SingleThreadedExecutor * - rclcpp::executors::SingleThreadedExecutor::add_node() diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 3434f473c6..4760839905 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -220,6 +220,26 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); } +// Check executor exits immediately if condition is complete. +TYPED_TEST(TestExecutors, testSpinUntilCompleteCallable) +{ + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // test success of an immediately completed condition + auto condition = []() {return true;}; + + // spin_until_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto start = std::chrono::steady_clock::now(); + auto ret = executor.spin_until_complete(condition, 1s); + executor.remove_node(this->node, true); + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + // Same test, but uses a shared future. TYPED_TEST(TestExecutors, testSpinUntilSharedFutureComplete) { diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 668ab96797..cac24239fb 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -283,6 +283,37 @@ TEST_F(TestExecutor, spin_some_elapsed) { ASSERT_TRUE(timer_called); } +TEST_F(TestExecutor, spin_for_duration) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(0), [&]() { + timer_called = true; + }); + dummy.add_node(node); + // Wait for the wall timer to have expired. + dummy.spin_for(std::chrono::milliseconds(0)); + + ASSERT_TRUE(timer_called); +} + +TEST_F(TestExecutor, spin_for_longer_timer) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::seconds(10), [&]() { + timer_called = true; + }); + dummy.add_node(node); + dummy.spin_for(std::chrono::milliseconds(5)); + + ASSERT_FALSE(timer_called); +} + TEST_F(TestExecutor, spin_once_in_spin_once) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); @@ -391,9 +422,7 @@ TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) { std::future future = promise.get_future(); dummy.spin_until_future_complete(future, std::chrono::milliseconds(1)); } catch (const std::runtime_error & err) { - if (err.what() == std::string( - "spin_until_future_complete() called while already spinning")) - { + if (err.what() == std::string("spin_until_complete() called while already spinning")) { spin_until_future_complete_in_spin_until_future_complete = true; } } @@ -488,6 +517,37 @@ TEST_F(TestExecutor, spin_until_future_complete_future_already_complete) { dummy.spin_until_future_complete(future, std::chrono::milliseconds(1))); } +TEST_F(TestExecutor, spin_until_complete_condition_already_complete) { + DummyExecutor dummy; + auto condition = []() {return true;}; + EXPECT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + dummy.spin_until_complete(condition, std::chrono::milliseconds(1))); +} + +TEST_F(TestExecutor, spin_until_complete_returns_after_condition) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + dummy.add_node(node); + // Check that we stop spinning after the condition is ready + const auto condition_delay = std::chrono::milliseconds(10); + const auto start = std::chrono::steady_clock::now(); + auto condition = [&]() {return std::chrono::steady_clock::now() - start > condition_delay;}; + EXPECT_EQ( + rclcpp::FutureReturnCode::SUCCESS, + dummy.spin_until_complete(condition, std::chrono::seconds(1))); + EXPECT_TRUE(spin_called); + const auto end_delay = std::chrono::steady_clock::now() - start; + EXPECT_GE(end_delay, condition_delay); + EXPECT_LT(end_delay, std::chrono::milliseconds(500)); +} + TEST_F(TestExecutor, is_spinning) { DummyExecutor dummy; ASSERT_FALSE(dummy.is_spinning());