Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add support for spin_until_complete (take 2) #2475

Open
wants to merge 5 commits into
base: rolling
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
78 changes: 53 additions & 25 deletions rclcpp/include/rclcpp/executor.hpp
Original file line number Diff line number Diff line change
@@ -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<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
FutureReturnCode
spin_until_future_complete(
const FutureT & future,
template<typename TimeRepT = int64_t, typename TimeT = std::milli>
SpinUntilCompleteReturnCode
spin_until_complete(
const std::function<bool(void)> & condition,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-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<std::chrono::nanoseconds>(
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<std::chrono::nanoseconds>(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<typename TimeRepT = int64_t, typename TimeT = std::milli>
void
spin_for(std::chrono::duration<TimeRepT, TimeT> 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<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
FutureReturnCode
spin_until_future_complete(
const FutureT & future,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-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.
66 changes: 66 additions & 0 deletions rclcpp/include/rclcpp/executors.hpp
Original file line number Diff line number Diff line change
@@ -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<typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::SpinUntilCompleteReturnCode
spin_node_until_complete(
rclcpp::Executor & executor,
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const std::function<bool(void)> & condition,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-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<typename NodeT = rclcpp::Node, typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::SpinUntilCompleteReturnCode
spin_node_until_complete(
rclcpp::Executor & executor,
std::shared_ptr<NodeT> node_ptr,
const std::function<bool(void)> & condition,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-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<typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::SpinUntilCompleteReturnCode
spin_until_complete(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
const std::function<bool(void)> & condition,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
rclcpp::executors::SingleThreadedExecutor executor;
return executors::spin_node_until_complete(executor, node_ptr, condition, timeout);
}

template<typename NodeT = rclcpp::Node, typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::SpinUntilCompleteReturnCode
spin_until_complete(
std::shared_ptr<NodeT> node_ptr,
const std::function<bool(void)> & condition,
std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
{
return rclcpp::spin_until_complete(node_ptr->get_node_base_interface(), condition, timeout);
}

template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
rclcpp::FutureReturnCode
spin_until_future_complete(
13 changes: 2 additions & 11 deletions rclcpp/include/rclcpp/future_return_code.hpp
Original file line number Diff line number Diff line change
@@ -18,6 +18,7 @@
#include <iostream>
#include <string>

#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

1 change: 1 addition & 0 deletions rclcpp/include/rclcpp/rclcpp.hpp
Original file line number Diff line number Diff line change
@@ -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()
47 changes: 47 additions & 0 deletions rclcpp/include/rclcpp/spin_until_complete_return_code.hpp
Original file line number Diff line number Diff line change
@@ -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 <iostream>
#include <string>

#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_
Original file line number Diff line number Diff line change
@@ -16,31 +16,34 @@
#include <string>
#include <type_traits>

#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<FutureReturnCode>::type;
using enum_type = std::underlying_type<SpinUntilCompleteReturnCode>::type;
std::string prefix = "Unknown enum value (";
std::string ret_as_string = std::to_string(static_cast<enum_type>(future_return_code));
switch (future_return_code) {
case FutureReturnCode::SUCCESS:
std::string ret_as_string = std::to_string(
static_cast<enum_type>(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;
}
6 changes: 6 additions & 0 deletions rclcpp/test/rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
20 changes: 20 additions & 0 deletions rclcpp/test/rclcpp/executors/test_executors.cpp
Original file line number Diff line number Diff line change
@@ -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)
{
Loading