From 36d56c5794ea46c72c1efd88a4f786f0247f23d1 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Tue, 2 Apr 2024 10:55:36 -0700 Subject: [PATCH 1/5] Add spin_until_complete and spin_for Co-authored-by: Hubert Liberacki Co-authored-by: Tomoya Fujita Signed-off-by: Hubert Liberacki Signed-off-by: Tomoya Fujita Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/executor.hpp | 68 +++++++++++++------ rclcpp/include/rclcpp/executors.hpp | 63 +++++++++++++++++ rclcpp/include/rclcpp/rclcpp.hpp | 1 + .../test/rclcpp/executors/test_executors.cpp | 20 ++++++ rclcpp/test/rclcpp/test_executor.cpp | 66 +++++++++++++++++- 5 files changed, 194 insertions(+), 24 deletions(-) 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()); From 0fb80463cc9d954571c976cba54072b08d75d7b5 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 4 Apr 2024 15:35:25 -0700 Subject: [PATCH 2/5] Add ConditionWaitReturnCode for spin_until_complete Signed-off-by: Christophe Bedard --- rclcpp/CMakeLists.txt | 2 +- .../rclcpp/condition_wait_return_code.hpp | 47 +++++++++++++++++++ rclcpp/include/rclcpp/executor.hpp | 13 ++--- rclcpp/include/rclcpp/executors.hpp | 10 ++-- rclcpp/include/rclcpp/future_return_code.hpp | 13 +---- ...ode.cpp => condition_wait_return_code.cpp} | 20 ++++---- rclcpp/test/rclcpp/CMakeLists.txt | 6 +++ .../test_condition_wait_return_code.cpp | 42 +++++++++++++++++ 8 files changed, 121 insertions(+), 32 deletions(-) create mode 100644 rclcpp/include/rclcpp/condition_wait_return_code.hpp rename rclcpp/src/rclcpp/{future_return_code.cpp => condition_wait_return_code.cpp} (65%) create mode 100644 rclcpp/test/rclcpp/test_condition_wait_return_code.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c95df3e768..4e4706ef76 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -43,6 +43,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/callback_group.cpp src/rclcpp/client.cpp src/rclcpp/clock.cpp + src/rclcpp/condition_wait_return_code.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp src/rclcpp/create_generic_client.cpp @@ -73,7 +74,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/expand_topic_or_service_name.cpp src/rclcpp/experimental/executors/events_executor/events_executor.cpp src/rclcpp/experimental/timers_manager.cpp - src/rclcpp/future_return_code.cpp src/rclcpp/generic_client.cpp src/rclcpp/generic_publisher.cpp src/rclcpp/generic_subscription.cpp diff --git a/rclcpp/include/rclcpp/condition_wait_return_code.hpp b/rclcpp/include/rclcpp/condition_wait_return_code.hpp new file mode 100644 index 0000000000..2aa863644d --- /dev/null +++ b/rclcpp/include/rclcpp/condition_wait_return_code.hpp @@ -0,0 +1,47 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ +#define RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ + +#include +#include + +#include "rclcpp/visibility_control.hpp" + +namespace rclcpp +{ + +/// Return codes to be used with spin_until_complete. +/** + * SUCCESS: The condition wait is complete. This does not indicate that the operation succeeded. + * INTERRUPTED: The condition wait is not complete, spinning was interrupted by Ctrl-C or another + * error. + * TIMEOUT: Spinning timed out. + */ +enum class ConditionWaitReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +/// Stream operator for ConditionWaitReturnCode. +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const ConditionWaitReturnCode & condition_wait_return_code); + +/// String conversion function for ConditionWaitReturnCode. +RCLCPP_PUBLIC +std::string +to_string(const ConditionWaitReturnCode & condition_wait_return_code); + +} // namespace rclcpp + +#endif // RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index b9bfb52c45..fc7df14371 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -32,6 +32,7 @@ #include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" +#include "rclcpp/condition_wait_return_code.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" @@ -361,7 +362,7 @@ class Executor * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template - FutureReturnCode + ConditionWaitReturnCode spin_until_complete( const std::function & condition, std::chrono::duration timeout = std::chrono::duration(-1)) @@ -376,7 +377,7 @@ class Executor // Preliminary check, finish if condition is done already. if (condition()) { - return FutureReturnCode::SUCCESS; + return ConditionWaitReturnCode::SUCCESS; } if (spinning.exchange(true)) { @@ -388,7 +389,7 @@ class Executor spin_once_impl(timeout_left); if (condition()) { - return FutureReturnCode::SUCCESS; + return ConditionWaitReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. if (timeout_ns < std::chrono::nanoseconds::zero()) { @@ -397,14 +398,14 @@ class Executor // 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 FutureReturnCode::TIMEOUT; + return ConditionWaitReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. timeout_left = std::chrono::duration_cast(end_time - now); } // The condition did not pass before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; + return ConditionWaitReturnCode::INTERRUPTED; } /// Spin (blocking) for at least the given amount of duration. @@ -413,7 +414,7 @@ class Executor */ template void - spin_for(std::chrono::duration timeout duration) + spin_for(std::chrono::duration duration) { (void)spin_until_complete([]() {return false;}, duration); } diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 92d6903856..c40ad7c884 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -18,10 +18,12 @@ #include #include +#include "rclcpp/condition_wait_return_code.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/experimental/executors/events_executor/events_executor.hpp" +#include "rclcpp/future_return_code.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -79,7 +81,7 @@ using rclcpp::executors::SingleThreadedExecutor; * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template -rclcpp::FutureReturnCode +rclcpp::ConditionWaitReturnCode spin_node_until_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, @@ -95,7 +97,7 @@ spin_node_until_complete( } template -rclcpp::FutureReturnCode +rclcpp::ConditionWaitReturnCode spin_node_until_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, @@ -156,7 +158,7 @@ spin_node_until_future_complete( } // namespace executors template -rclcpp::FutureReturnCode +rclcpp::ConditionWaitReturnCode spin_until_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const std::function & condition, @@ -167,7 +169,7 @@ spin_until_complete( } template -rclcpp::FutureReturnCode +rclcpp::ConditionWaitReturnCode spin_until_complete( std::shared_ptr node_ptr, const std::function & condition, diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp index 0da67d7f7b..34fdcf7163 100644 --- a/rclcpp/include/rclcpp/future_return_code.hpp +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -18,6 +18,7 @@ #include #include +#include "rclcpp/condition_wait_return_code.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -30,17 +31,7 @@ namespace rclcpp * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. * TIMEOUT: Spinning timed out. */ -enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; - -/// Stream operator for FutureReturnCode. -RCLCPP_PUBLIC -std::ostream & -operator<<(std::ostream & os, const FutureReturnCode & future_return_code); - -/// String conversion function for FutureReturnCode. -RCLCPP_PUBLIC -std::string -to_string(const FutureReturnCode & future_return_code); +using FutureReturnCode = ConditionWaitReturnCode; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/future_return_code.cpp b/rclcpp/src/rclcpp/condition_wait_return_code.cpp similarity index 65% rename from rclcpp/src/rclcpp/future_return_code.cpp rename to rclcpp/src/rclcpp/condition_wait_return_code.cpp index 61e167dfb9..ff9b4f4920 100644 --- a/rclcpp/src/rclcpp/future_return_code.cpp +++ b/rclcpp/src/rclcpp/condition_wait_return_code.cpp @@ -16,31 +16,31 @@ #include #include -#include "rclcpp/future_return_code.hpp" +#include "rclcpp/condition_wait_return_code.hpp" namespace rclcpp { std::ostream & -operator<<(std::ostream & os, const rclcpp::FutureReturnCode & future_return_code) +operator<<(std::ostream & os, const rclcpp::ConditionWaitReturnCode & condition_wait_return_code) { - return os << to_string(future_return_code); + return os << to_string(condition_wait_return_code); } std::string -to_string(const rclcpp::FutureReturnCode & future_return_code) +to_string(const rclcpp::ConditionWaitReturnCode & condition_wait_return_code) { - using enum_type = std::underlying_type::type; + using enum_type = std::underlying_type::type; std::string prefix = "Unknown enum value ("; - std::string ret_as_string = std::to_string(static_cast(future_return_code)); - switch (future_return_code) { - case FutureReturnCode::SUCCESS: + std::string ret_as_string = std::to_string(static_cast(condition_wait_return_code)); + switch (condition_wait_return_code) { + case ConditionWaitReturnCode::SUCCESS: prefix = "SUCCESS ("; break; - case FutureReturnCode::INTERRUPTED: + case ConditionWaitReturnCode::INTERRUPTED: prefix = "INTERRUPTED ("; break; - case FutureReturnCode::TIMEOUT: + case ConditionWaitReturnCode::TIMEOUT: prefix = "TIMEOUT ("; break; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c2e6b2bfe4..39ea50fec8 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -109,6 +109,12 @@ ament_add_gtest(test_function_traits test_function_traits.cpp) if(TARGET test_function_traits) target_link_libraries(test_function_traits ${PROJECT_NAME}) endif() +ament_add_gtest( + test_condition_wait_return_code + test_condition_wait_return_code.cpp) +if(TARGET test_condition_wait_return_code) + target_link_libraries(test_condition_wait_return_code ${PROJECT_NAME}) +endif() ament_add_gtest( test_future_return_code test_future_return_code.cpp) diff --git a/rclcpp/test/rclcpp/test_condition_wait_return_code.cpp b/rclcpp/test/rclcpp/test_condition_wait_return_code.cpp new file mode 100644 index 0000000000..3db4f09fcc --- /dev/null +++ b/rclcpp/test/rclcpp/test_condition_wait_return_code.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/condition_wait_return_code.hpp" + +TEST(TestConditionWaitReturnCode, to_string) { + EXPECT_EQ( + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(-1))); + EXPECT_EQ( + "SUCCESS (0)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::SUCCESS)); + EXPECT_EQ( + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::INTERRUPTED)); + EXPECT_EQ( + "TIMEOUT (2)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::TIMEOUT)); + EXPECT_EQ( + "Unknown enum value (3)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(3))); + EXPECT_EQ( + "Unknown enum value (100)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(100))); +} + +TEST(TestConditionWaitReturnCode, ostream) { + std::ostringstream ostream; + + ostream << rclcpp::ConditionWaitReturnCode::SUCCESS; + ASSERT_EQ("SUCCESS (0)", ostream.str()); +} From bc06d38fd04c704d71ad1ae4059732df7ad9f5a5 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Tue, 9 Apr 2024 08:42:32 -0700 Subject: [PATCH 3/5] Document condition possibly leading to function infinitely blocking Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/executor.hpp | 3 ++- rclcpp/include/rclcpp/executors.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index fc7df14371..bc421d45af 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -354,7 +354,8 @@ class Executor /// Spin (blocking) until the condition is complete, it times out waiting, or rclcpp is /// interrupted. /** - * \param[in] condition The callable condition to wait on. + * \param[in] condition The callable condition to wait on. If this condition is not related to + * what the executor is waiting on and the timeout is infinite, this could block forever. * \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 diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index c40ad7c884..8ba6ebdd36 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -73,7 +73,8 @@ using rclcpp::executors::SingleThreadedExecutor; /** * \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] condition The callable condition to wait on. If this condition is not related to + * what the executor is waiting on and the timeout is infinite, this could block forever. * \param[in] timeout Optional timeout parameter, which gets passed to * Executor::spin_node_once. * `-1` is block forever, `0` is non-blocking. From c76d80694aa4e5c5893e5a2287cbb67284c4312b Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Tue, 9 Apr 2024 10:32:36 -0700 Subject: [PATCH 4/5] Fix test Signed-off-by: Christophe Bedard --- rclcpp/test/rclcpp/test_executor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index cac24239fb..904582273e 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -528,11 +528,11 @@ TEST_F(TestExecutor, spin_until_complete_condition_already_complete) { TEST_F(TestExecutor, spin_until_complete_returns_after_condition) { DummyExecutor dummy; auto node = std::make_shared("node", "ns"); - bool spin_called = false; + bool timer_called = false; auto timer = node->create_wall_timer( std::chrono::milliseconds(1), [&]() { - spin_called = true; + timer_called = true; }); dummy.add_node(node); // Check that we stop spinning after the condition is ready @@ -542,10 +542,9 @@ TEST_F(TestExecutor, spin_until_complete_returns_after_condition) { EXPECT_EQ( rclcpp::FutureReturnCode::SUCCESS, dummy.spin_until_complete(condition, std::chrono::seconds(1))); - EXPECT_TRUE(spin_called); + EXPECT_TRUE(timer_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) { From 3ed838fb224e0738db4e7238ae0eb6184c27cdfc Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Wed, 10 Apr 2024 09:06:18 -0700 Subject: [PATCH 5/5] Rename ConditionWaitReturnCode to SpinUntilCompleteReturnCode Signed-off-by: Christophe Bedard --- rclcpp/CMakeLists.txt | 2 +- rclcpp/include/rclcpp/executor.hpp | 12 +++++----- rclcpp/include/rclcpp/executors.hpp | 10 ++++---- rclcpp/include/rclcpp/future_return_code.hpp | 4 ++-- ...pp => spin_until_complete_return_code.hpp} | 16 ++++++------- ...pp => spin_until_complete_return_code.cpp} | 23 +++++++++++-------- rclcpp/test/rclcpp/CMakeLists.txt | 8 +++---- ... test_spin_until_complete_return_code.cpp} | 20 ++++++++-------- 8 files changed, 49 insertions(+), 46 deletions(-) rename rclcpp/include/rclcpp/{condition_wait_return_code.hpp => spin_until_complete_return_code.hpp} (67%) rename rclcpp/src/rclcpp/{condition_wait_return_code.cpp => spin_until_complete_return_code.cpp} (58%) rename rclcpp/test/rclcpp/{test_condition_wait_return_code.cpp => test_spin_until_complete_return_code.cpp} (52%) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 4e4706ef76..2305d2fdfc 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -43,7 +43,6 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/callback_group.cpp src/rclcpp/client.cpp src/rclcpp/clock.cpp - src/rclcpp/condition_wait_return_code.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp src/rclcpp/create_generic_client.cpp @@ -116,6 +115,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp src/rclcpp/signal_handler.cpp + src/rclcpp/spin_until_complete_return_code.cpp src/rclcpp/subscription_base.cpp src/rclcpp/subscription_intra_process_base.cpp src/rclcpp/time.cpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index bc421d45af..e23c0ad857 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -32,7 +32,6 @@ #include "rclcpp/executors/executor_notify_waitable.hpp" #include "rcpputils/scope_exit.hpp" -#include "rclcpp/condition_wait_return_code.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" @@ -41,6 +40,7 @@ #include "rclcpp/executors/executor_entities_collector.hpp" #include "rclcpp/future_return_code.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/wait_set.hpp" @@ -363,7 +363,7 @@ class Executor * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template - ConditionWaitReturnCode + SpinUntilCompleteReturnCode spin_until_complete( const std::function & condition, std::chrono::duration timeout = std::chrono::duration(-1)) @@ -378,7 +378,7 @@ class Executor // Preliminary check, finish if condition is done already. if (condition()) { - return ConditionWaitReturnCode::SUCCESS; + return SpinUntilCompleteReturnCode::SUCCESS; } if (spinning.exchange(true)) { @@ -390,7 +390,7 @@ class Executor spin_once_impl(timeout_left); if (condition()) { - return ConditionWaitReturnCode::SUCCESS; + return SpinUntilCompleteReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. if (timeout_ns < std::chrono::nanoseconds::zero()) { @@ -399,14 +399,14 @@ class Executor // 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 ConditionWaitReturnCode::TIMEOUT; + return SpinUntilCompleteReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. timeout_left = std::chrono::duration_cast(end_time - now); } // The condition did not pass before ok() returned false, return INTERRUPTED. - return ConditionWaitReturnCode::INTERRUPTED; + return SpinUntilCompleteReturnCode::INTERRUPTED; } /// Spin (blocking) for at least the given amount of duration. diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 8ba6ebdd36..3372ac40c8 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -18,13 +18,13 @@ #include #include -#include "rclcpp/condition_wait_return_code.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/experimental/executors/events_executor/events_executor.hpp" #include "rclcpp/future_return_code.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" @@ -82,7 +82,7 @@ using rclcpp::executors::SingleThreadedExecutor; * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ template -rclcpp::ConditionWaitReturnCode +rclcpp::SpinUntilCompleteReturnCode spin_node_until_complete( rclcpp::Executor & executor, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, @@ -98,7 +98,7 @@ spin_node_until_complete( } template -rclcpp::ConditionWaitReturnCode +rclcpp::SpinUntilCompleteReturnCode spin_node_until_complete( rclcpp::Executor & executor, std::shared_ptr node_ptr, @@ -159,7 +159,7 @@ spin_node_until_future_complete( } // namespace executors template -rclcpp::ConditionWaitReturnCode +rclcpp::SpinUntilCompleteReturnCode spin_until_complete( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr, const std::function & condition, @@ -170,7 +170,7 @@ spin_until_complete( } template -rclcpp::ConditionWaitReturnCode +rclcpp::SpinUntilCompleteReturnCode spin_until_complete( std::shared_ptr node_ptr, const std::function & condition, diff --git a/rclcpp/include/rclcpp/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp index 34fdcf7163..9012d78e94 100644 --- a/rclcpp/include/rclcpp/future_return_code.hpp +++ b/rclcpp/include/rclcpp/future_return_code.hpp @@ -18,7 +18,7 @@ #include #include -#include "rclcpp/condition_wait_return_code.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -31,7 +31,7 @@ namespace rclcpp * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. * TIMEOUT: Spinning timed out. */ -using FutureReturnCode = ConditionWaitReturnCode; +using FutureReturnCode = SpinUntilCompleteReturnCode; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/condition_wait_return_code.hpp b/rclcpp/include/rclcpp/spin_until_complete_return_code.hpp similarity index 67% rename from rclcpp/include/rclcpp/condition_wait_return_code.hpp rename to rclcpp/include/rclcpp/spin_until_complete_return_code.hpp index 2aa863644d..aca09cbe64 100644 --- a/rclcpp/include/rclcpp/condition_wait_return_code.hpp +++ b/rclcpp/include/rclcpp/spin_until_complete_return_code.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ -#define RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ +#ifndef RCLCPP__SPIN_UNTIL_COMPLETE_RETURN_CODE_HPP_ +#define RCLCPP__SPIN_UNTIL_COMPLETE_RETURN_CODE_HPP_ #include #include @@ -30,18 +30,18 @@ namespace rclcpp * error. * TIMEOUT: Spinning timed out. */ -enum class ConditionWaitReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; +enum class SpinUntilCompleteReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; -/// Stream operator for ConditionWaitReturnCode. +/// Stream operator for SpinUntilCompleteReturnCode. RCLCPP_PUBLIC std::ostream & -operator<<(std::ostream & os, const ConditionWaitReturnCode & condition_wait_return_code); +operator<<(std::ostream & os, const SpinUntilCompleteReturnCode & spin_until_complete_return_code); -/// String conversion function for ConditionWaitReturnCode. +/// String conversion function for SpinUntilCompleteReturnCode. RCLCPP_PUBLIC std::string -to_string(const ConditionWaitReturnCode & condition_wait_return_code); +to_string(const SpinUntilCompleteReturnCode & spin_until_complete_return_code); } // namespace rclcpp -#endif // RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_ +#endif // RCLCPP__SPIN_UNTIL_COMPLETE_RETURN_CODE_HPP_ diff --git a/rclcpp/src/rclcpp/condition_wait_return_code.cpp b/rclcpp/src/rclcpp/spin_until_complete_return_code.cpp similarity index 58% rename from rclcpp/src/rclcpp/condition_wait_return_code.cpp rename to rclcpp/src/rclcpp/spin_until_complete_return_code.cpp index ff9b4f4920..eaa9bdc2e2 100644 --- a/rclcpp/src/rclcpp/condition_wait_return_code.cpp +++ b/rclcpp/src/rclcpp/spin_until_complete_return_code.cpp @@ -16,31 +16,34 @@ #include #include -#include "rclcpp/condition_wait_return_code.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" namespace rclcpp { std::ostream & -operator<<(std::ostream & os, const rclcpp::ConditionWaitReturnCode & condition_wait_return_code) +operator<<( + std::ostream & os, + const rclcpp::SpinUntilCompleteReturnCode & spin_until_complete_return_code) { - return os << to_string(condition_wait_return_code); + return os << to_string(spin_until_complete_return_code); } std::string -to_string(const rclcpp::ConditionWaitReturnCode & condition_wait_return_code) +to_string(const rclcpp::SpinUntilCompleteReturnCode & spin_until_complete_return_code) { - using enum_type = std::underlying_type::type; + using enum_type = std::underlying_type::type; std::string prefix = "Unknown enum value ("; - std::string ret_as_string = std::to_string(static_cast(condition_wait_return_code)); - switch (condition_wait_return_code) { - case ConditionWaitReturnCode::SUCCESS: + std::string ret_as_string = std::to_string( + static_cast(spin_until_complete_return_code)); + switch (spin_until_complete_return_code) { + case SpinUntilCompleteReturnCode::SUCCESS: prefix = "SUCCESS ("; break; - case ConditionWaitReturnCode::INTERRUPTED: + case SpinUntilCompleteReturnCode::INTERRUPTED: prefix = "INTERRUPTED ("; break; - case ConditionWaitReturnCode::TIMEOUT: + case SpinUntilCompleteReturnCode::TIMEOUT: prefix = "TIMEOUT ("; break; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 39ea50fec8..6bd714fd77 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -110,10 +110,10 @@ if(TARGET test_function_traits) target_link_libraries(test_function_traits ${PROJECT_NAME}) endif() ament_add_gtest( - test_condition_wait_return_code - test_condition_wait_return_code.cpp) -if(TARGET test_condition_wait_return_code) - target_link_libraries(test_condition_wait_return_code ${PROJECT_NAME}) + test_spin_until_complete_return_code + test_spin_until_complete_return_code.cpp) +if(TARGET test_spin_until_complete_return_code) + target_link_libraries(test_spin_until_complete_return_code ${PROJECT_NAME}) endif() ament_add_gtest( test_future_return_code diff --git a/rclcpp/test/rclcpp/test_condition_wait_return_code.cpp b/rclcpp/test/rclcpp/test_spin_until_complete_return_code.cpp similarity index 52% rename from rclcpp/test/rclcpp/test_condition_wait_return_code.cpp rename to rclcpp/test/rclcpp/test_spin_until_complete_return_code.cpp index 3db4f09fcc..eecee5fd3b 100644 --- a/rclcpp/test/rclcpp/test_condition_wait_return_code.cpp +++ b/rclcpp/test/rclcpp/test_spin_until_complete_return_code.cpp @@ -17,26 +17,26 @@ #include #include -#include "rclcpp/condition_wait_return_code.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" -TEST(TestConditionWaitReturnCode, to_string) { +TEST(TestSpinUntilCompleteReturnCode, to_string) { EXPECT_EQ( - "Unknown enum value (-1)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(-1))); + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(-1))); EXPECT_EQ( - "SUCCESS (0)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::SUCCESS)); + "SUCCESS (0)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::SUCCESS)); EXPECT_EQ( - "INTERRUPTED (1)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::INTERRUPTED)); + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::INTERRUPTED)); EXPECT_EQ( - "TIMEOUT (2)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode::TIMEOUT)); + "TIMEOUT (2)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::TIMEOUT)); EXPECT_EQ( - "Unknown enum value (3)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(3))); + "Unknown enum value (3)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(3))); EXPECT_EQ( - "Unknown enum value (100)", rclcpp::to_string(rclcpp::ConditionWaitReturnCode(100))); + "Unknown enum value (100)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(100))); } -TEST(TestConditionWaitReturnCode, ostream) { +TEST(TestSpinUntilCompleteReturnCode, ostream) { std::ostringstream ostream; - ostream << rclcpp::ConditionWaitReturnCode::SUCCESS; + ostream << rclcpp::SpinUntilCompleteReturnCode::SUCCESS; ASSERT_EQ("SUCCESS (0)", ostream.str()); }