DRAFT: Add support for spin_for(timeout) and refactor spin_until_feat…

…ure_complete (#1821)

Signed-off-by: Hubert Liberacki [email protected]

rclcpp/include/rclcpp/executor.hpp

@@ -26,6 +26,7 @@
 #include <mutex>
 #include <string>
 #include <vector>
+#include <type_traits>
 
 #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<typename FutureT, typename DurationT = std::chrono::milliseconds,
+ // typename std::enable_if_t<
+ // std::is_same_v<
+ // std::remove_reference_t<FutureT>,
+ // std::future<
+ // decltype(std::declval<std::remove_reference_t<FutureT>>()
+ // .get())>> ||
+ // std::is_same_v<
+ // std::remove_reference_t<FutureT>,
+ // std::shared_future<
+ // decltype(std::declval<std::remove_reference_t<FutureT>>()
+ // .get())>>,
+ // bool> = true>
+ typename std::enable_if_t<!std::is_invocable_v<FutureT>, 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<std::is_invocable_v<Condition>, bool> = true>
+ FutureReturnCode spin_until_complete(
+ Condition & condition,
+ DurationT timeout = DurationT(-1))
+ {
+ using RetT = decltype(std::declval<std::remove_reference_t<Condition>>()());
+ static_assert(
+ std::is_same_v<bool, RetT>,
+ "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<typename DurationT>
+ 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<typename Condition, typename DurationT>
+ 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<std::chrono::nanoseconds>(
+ 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<std::chrono::nanoseconds>(end_time - now);
+ }
+
+ // The condition did not pass before ok() returned false, return INTERRUPTED.
+ return FutureReturnCode::INTERRUPTED;
+ }
+
+public:
  typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
  const rclcpp::GuardCondition *,
  std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>