Add spin_until_complete and spin_for

Co-authored-by: Hubert Liberacki <hliberacki@gmail.com>
Co-authored-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Signed-off-by: Hubert Liberacki <hliberacki@gmail.com>
Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Signed-off-by: Christophe Bedard <christophe.bedard@apex.ai>

Author: 3 people

rclcpp/include/rclcpp/executor.hpp

@@ -350,32 +350,22 @@ 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.
    * \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>
+  template<typename TimeRepT = int64_t, typename TimeT = std::milli>
   FutureReturnCode
-  spin_until_future_complete(
-    const FutureT & future,
+  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,17 +374,20 @@ class Executor
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
+    // Preliminary check, finish if condition is done already.
+    if (condition()) {
+      return FutureReturnCode::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) {
+      if (condition()) {
         return FutureReturnCode::SUCCESS;
       }
       // If the original timeout is < 0, then this is blocking, never TIMEOUT.
@@ -410,10 +403,43 @@ class Executor
       timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
     }
 
-    // The future did not complete before ok() returned false, return INTERRUPTED.
+    // The condition did not pass before ok() returned false, return INTERRUPTED.
     return FutureReturnCode::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> timeout 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.
   /**
    * This function can be called asynchonously from any thread.

rclcpp/include/rclcpp/executors.hpp

@@ -67,6 +67,48 @@ 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.
+ * \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::FutureReturnCode
+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::FutureReturnCode
+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 +155,27 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
+template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+rclcpp::FutureReturnCode
+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::FutureReturnCode
+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(

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

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

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<rclcpp::Node>("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<rclcpp::Node>("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<rclcpp::Node>("node", "ns");
@@ -391,9 +422,7 @@ TEST_F(TestExecutor, spin_until_future_complete_in_spin_until_future_complete) {
         std::future<void> 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,37 @@ 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<rclcpp::Node>("node", "ns");
+  bool spin_called = false;
+  auto timer =
+    node->create_wall_timer(
+    std::chrono::milliseconds(1), [&]() {
+      spin_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(spin_called);
+  const auto end_delay = std::chrono::steady_clock::now() - start;
+  EXPECT_GE(end_delay, condition_delay);
+  EXPECT_LT(end_delay, std::chrono::milliseconds(500));
+}
+
 TEST_F(TestExecutor, is_spinning) {
   DummyExecutor dummy;
   ASSERT_FALSE(dummy.is_spinning());