diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index ed2ddc4a0a..c303a1597b 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include "rcl/guard_condition.h" #include "rcl/wait.h" @@ -319,6 +320,53 @@ class Executor virtual void spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + // TODO(hliberacki): Add documentation + template, + // std::future< + // decltype(std::declval>() + // .get())>> || + // std::is_same_v< + // std::remove_reference_t, + // std::shared_future< + // decltype(std::declval>() + // .get())>>, + // bool> = true> + typename std::enable_if_t, bool> = true> + FutureReturnCode spin_until_complete(FutureT & future, DurationT timeout = DurationT(-1)) + { + // call helper wait untill complete with lambda + auto checkFuture = [&future]() { + return future.wait_for(std::chrono::seconds(0)) == + std::future_status::ready; + }; + return spin_until_complete_impl(checkFuture, timeout); + } + + template< + typename Condition, typename DurationT = std::chrono::milliseconds, + typename std::enable_if_t, bool> = true> + FutureReturnCode spin_until_complete( + Condition & condition, + DurationT timeout = DurationT(-1)) + { + using RetT = decltype(std::declval>()()); + static_assert( + std::is_same_v, + "Conditional callable has to return boolean type"); + return spin_until_complete_impl(condition, timeout); + } + // TODO(hliberacki): Add documentation + /// spin (blocking) for given amount of time + template + void + spin_for(DurationT timeout) + { + return spin_until_complete([]() {return false;}, timeout); + } + /// 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 @@ -560,6 +608,46 @@ class Executor virtual void spin_once_impl(std::chrono::nanoseconds timeout); +private: + template + FutureReturnCode spin_until_complete_impl(Condition condition, DurationT timeout) + { + auto end_time = std::chrono::steady_clock::now(); + std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast( + timeout); + if (timeout_ns > std::chrono::nanoseconds::zero()) { + end_time += timeout_ns; + } + std::chrono::nanoseconds timeout_left = timeout_ns; + if (spinning.exchange(true)) { + 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); + + if (condition()) { + return FutureReturnCode::SUCCESS; + } + // If the original timeout is < 0, then this is blocking, never TIMEOUT. + if (timeout_ns < std::chrono::nanoseconds::zero()) { + continue; + } + // 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; + } + // 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; + } + +public: typedef std::map>