DRAFT: Add support for spin_for(timeout) and refactor spin_until_feature_complete (#1821)

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

Author: hliberacki

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>>