diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index c95df3e768..2305d2fdfc 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -73,7 +73,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 @@ -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 cfd7ea81fd..e23c0ad857 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -40,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" @@ -350,32 +351,23 @@ 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. 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 * code. * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. */ - template - FutureReturnCode - spin_until_future_complete( - const FutureT & future, + template + SpinUntilCompleteReturnCode + 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,18 +376,21 @@ class Executor } std::chrono::nanoseconds timeout_left = timeout_ns; + // Preliminary check, finish if condition is done already. + if (condition()) { + return SpinUntilCompleteReturnCode::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) { - return FutureReturnCode::SUCCESS; + if (condition()) { + return SpinUntilCompleteReturnCode::SUCCESS; } // If the original timeout is < 0, then this is blocking, never TIMEOUT. if (timeout_ns < std::chrono::nanoseconds::zero()) { @@ -404,14 +399,47 @@ 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 SpinUntilCompleteReturnCode::TIMEOUT; } // Subtract the elapsed time from the original timeout. timeout_left = std::chrono::duration_cast(end_time - now); } - // The future did not complete before ok() returned false, return INTERRUPTED. - return FutureReturnCode::INTERRUPTED; + // The condition did not pass before ok() returned false, return INTERRUPTED. + return SpinUntilCompleteReturnCode::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 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. diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 55a38709a7..3372ac40c8 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -22,7 +22,9 @@ #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" @@ -67,6 +69,49 @@ 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. 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 code. + * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`. + */ +template +rclcpp::SpinUntilCompleteReturnCode +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::SpinUntilCompleteReturnCode +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 +158,27 @@ spin_node_until_future_complete( } // namespace executors +template +rclcpp::SpinUntilCompleteReturnCode +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::SpinUntilCompleteReturnCode +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/future_return_code.hpp b/rclcpp/include/rclcpp/future_return_code.hpp index 0da67d7f7b..9012d78e94 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/spin_until_complete_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 = SpinUntilCompleteReturnCode; } // namespace rclcpp 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/include/rclcpp/spin_until_complete_return_code.hpp b/rclcpp/include/rclcpp/spin_until_complete_return_code.hpp new file mode 100644 index 0000000000..aca09cbe64 --- /dev/null +++ b/rclcpp/include/rclcpp/spin_until_complete_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__SPIN_UNTIL_COMPLETE_RETURN_CODE_HPP_ +#define RCLCPP__SPIN_UNTIL_COMPLETE_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 SpinUntilCompleteReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; + +/// Stream operator for SpinUntilCompleteReturnCode. +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const SpinUntilCompleteReturnCode & spin_until_complete_return_code); + +/// String conversion function for SpinUntilCompleteReturnCode. +RCLCPP_PUBLIC +std::string +to_string(const SpinUntilCompleteReturnCode & spin_until_complete_return_code); + +} // namespace rclcpp + +#endif // RCLCPP__SPIN_UNTIL_COMPLETE_RETURN_CODE_HPP_ diff --git a/rclcpp/src/rclcpp/future_return_code.cpp b/rclcpp/src/rclcpp/spin_until_complete_return_code.cpp similarity index 58% rename from rclcpp/src/rclcpp/future_return_code.cpp rename to rclcpp/src/rclcpp/spin_until_complete_return_code.cpp index 61e167dfb9..eaa9bdc2e2 100644 --- a/rclcpp/src/rclcpp/future_return_code.cpp +++ b/rclcpp/src/rclcpp/spin_until_complete_return_code.cpp @@ -16,31 +16,34 @@ #include #include -#include "rclcpp/future_return_code.hpp" +#include "rclcpp/spin_until_complete_return_code.hpp" namespace rclcpp { std::ostream & -operator<<(std::ostream & os, const rclcpp::FutureReturnCode & future_return_code) +operator<<( + std::ostream & os, + const rclcpp::SpinUntilCompleteReturnCode & spin_until_complete_return_code) { - return os << to_string(future_return_code); + return os << to_string(spin_until_complete_return_code); } std::string -to_string(const rclcpp::FutureReturnCode & future_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(future_return_code)); - switch (future_return_code) { - case FutureReturnCode::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 FutureReturnCode::INTERRUPTED: + case SpinUntilCompleteReturnCode::INTERRUPTED: prefix = "INTERRUPTED ("; break; - case FutureReturnCode::TIMEOUT: + case SpinUntilCompleteReturnCode::TIMEOUT: prefix = "TIMEOUT ("; break; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c2e6b2bfe4..6bd714fd77 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_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 test_future_return_code.cpp) 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..904582273e 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,36 @@ 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 timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + timer_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(timer_called); + const auto end_delay = std::chrono::steady_clock::now() - start; + EXPECT_GE(end_delay, condition_delay); +} + TEST_F(TestExecutor, is_spinning) { DummyExecutor dummy; ASSERT_FALSE(dummy.is_spinning()); diff --git a/rclcpp/test/rclcpp/test_spin_until_complete_return_code.cpp b/rclcpp/test/rclcpp/test_spin_until_complete_return_code.cpp new file mode 100644 index 0000000000..eecee5fd3b --- /dev/null +++ b/rclcpp/test/rclcpp/test_spin_until_complete_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/spin_until_complete_return_code.hpp" + +TEST(TestSpinUntilCompleteReturnCode, to_string) { + EXPECT_EQ( + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(-1))); + EXPECT_EQ( + "SUCCESS (0)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::SUCCESS)); + EXPECT_EQ( + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::INTERRUPTED)); + EXPECT_EQ( + "TIMEOUT (2)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode::TIMEOUT)); + EXPECT_EQ( + "Unknown enum value (3)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(3))); + EXPECT_EQ( + "Unknown enum value (100)", rclcpp::to_string(rclcpp::SpinUntilCompleteReturnCode(100))); +} + +TEST(TestSpinUntilCompleteReturnCode, ostream) { + std::ostringstream ostream; + + ostream << rclcpp::SpinUntilCompleteReturnCode::SUCCESS; + ASSERT_EQ("SUCCESS (0)", ostream.str()); +}