Add support for spin_until_timeout (ros2#1821)

* Created spin_until_timeout() method * Created spin_node_until_timeout() method * Extended unit tests Signed-off-by: Hubert Liberacki <[email protected]>
hliberacki · Jan 21, 2022 · 3a75f1a · 3a75f1a
1 parent 8afef51
commit 3a75f1a
Show file tree

Hide file tree

Showing 3 changed files with 203 additions and 5 deletions.
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
@@ -344,16 +344,18 @@ class Executor
     }
 
     auto end_time = std::chrono::steady_clock::now();
+
+    if (spinning.exchange(true)) {
+      throw std::runtime_error("spin_until_future_complete() called while already spinning");
+    }
+
     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_future_complete() called while already spinning");
-    }
     RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
     while (rclcpp::ok(this->context_) && spinning.load()) {
       // Do one item of work.
@@ -381,6 +383,47 @@ class Executor
     return FutureReturnCode::INTERRUPTED;
   }
 
+  /// Spin (blocking) until it times out, or rclcpp is interrupted.
+  /**
+   * \param[in] timeout Timeout parameter, which gets passed to Executor::spin_node_once.
+   * \throws std::runtime_error if spinning is already taking place.
+   */
+  template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+  void spin_until_timeout(
+    std::chrono::duration<TimeRepT, TimeT> timeout)
+  {
+    auto end_time = std::chrono::steady_clock::now();
+
+    if (spinning.exchange(true)) {
+      throw std::runtime_error("spin_until_timeout() called while already spinning");
+    }
+
+    std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      timeout);
+    if (timeout_ns <= std::chrono::nanoseconds::zero()) {
+      // No work to be done, timeout is reached.
+      return;
+    }
+
+    end_time += timeout_ns;
+    std::chrono::nanoseconds timeout_left = timeout_ns;
+
+    RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); );
+    while (rclcpp::ok(this->context_) && spinning.load()) {
+      // Do one item of work.
+      spin_once_impl(timeout_left);
+
+      // 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;
+      }
+
+      // Subtract the elapsed time from the original timeout.
+      timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
+    }
+  }
+
   /// Cancel any running spin* function, causing it to return.
   /**
    * This function can be called asynchonously from any thread.

diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp
@@ -54,6 +54,40 @@ namespace executors
 using rclcpp::executors::MultiThreadedExecutor;
 using rclcpp::executors::SingleThreadedExecutor;
 
+
+/// Spin (blocking) until it times out, or rclcpp is interrupted.
+/**
+ * \param[in] executor The executor which will spin the node.
+ * \param[in] node_ptr The node to spin.
+ * \param[in] timeout Timeout parameter, which gets passed to Executor::spin_node_once.
+ * \throws std::runtime_error if spinning is already taking place.
+ */
+template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+void
+spin_node_until_timeout(
+  rclcpp::Executor & executor,
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  executor.add_node(node_ptr);
+  executor.spin_until_timeout(timeout);
+  executor.remove_node(node_ptr);
+  return retcode;
+}
+
+template<typename NodeT = rclcpp::Node, typename TimeRepT = int64_t, typename TimeT = std::milli>
+void
+spin_node_until_timeout(
+  rclcpp::Executor & executor,
+  std::shared_ptr<NodeT> node_ptr,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  return rclcpp::executors::spin_until_timeout(
+    executor,
+    node_ptr->get_node_base_interface(),
+    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.
@@ -100,6 +134,26 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
+template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+void
+spin_until_timeout(
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  rclcpp::executors::SingleThreadedExecutor executor;
+  return executors::spin_node_until_timeout(executor, node_ptr, timeout);
+}
+
+template<typename NodeT = rclcpp::Node, typename TimeRepT = int64_t,
+  typename TimeT = std::milli>
+rclcpp::FutureReturnCode
+spin_until_timeout(
+  std::shared_ptr<NodeT> node_ptr,
+  std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
+{
+  return rclcpp::spin_node_until_timeout(node_ptr->get_node_base_interface(), timeout);
+}
+
 template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
 rclcpp::FutureReturnCode
 spin_until_future_complete(

diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp
@@ -221,6 +221,56 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) {
   executor.remove_node(this->node, true);
 }
 
+// Check if timeout is respected
+TYPED_TEST(TestExecutors, testSpinUntilTimeout) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // block intil timeout is fulfilled.
+  auto start = std::chrono::steady_clock::now();
+  executor.spin_until_timeout(2s);
+  executor.remove_node(this->node, true);
+  // Check if timeout was fulfilled
+  EXPECT_GT(2s, (std::chrono::steady_clock::now() - start));
+}
+
+// Check if throws while spinner is already active
+TYPED_TEST(TestExecutors, testSpinUntilTimeoutAlreadySpinning) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // Active spinner to check for collision.
+  std::thread spinner([&]() {
+      executor.spin_until_timeout(2s);
+    });
+
+  // There already is an active spinner running
+  // second call needs to throw.
+  EXCEPT_THROW(executor.spin_until_timeout(1ms));
+  executor.remove_node(this->node, true);
+  executor.cancel();
+  spinner.join();
+}
+
+// Check if timeout is respected
+TYPED_TEST(TestExecutors, testSpinUntilTimeoutNegativeTimeout) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+  executor.add_node(this->node);
+
+  // Block intil timeout is fulfilled.
+  auto start = std::chrono::steady_clock::now();
+
+  // Spinner will exit immediately
+  executor.spin_until_timeout(-1s);
+  // Check if timeout existed without waiting
+  // 100ms is an arbitrary picked duration
+  EXPECT_LT(100ms, (std::chrono::steady_clock::now() - start));
+  executor.remove_node(this->node, true);
+}
+
 // Check executor exits immediately if future is complete.
 TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
   using ExecutorType = TypeParam;
@@ -232,8 +282,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) {
   std::future<bool> future = promise.get_future();
   promise.set_value(true);
 
-  // spin_until_future_complete is expected to exit immediately, but would block up until its
-  // timeout if the future is not checked before spin_once_impl.
+  // block spin until timeout is reached
   auto start = std::chrono::steady_clock::now();
   auto shared_future = future.share();
   auto ret = executor.spin_until_future_complete(shared_future, 1s);
@@ -482,6 +531,58 @@ TYPED_TEST(TestExecutors, spinSome) {
   spinner.join();
 }
 
+// Check spin_node_until_timeout with node base pointer
+TYPED_TEST(TestExecutors, testSpinNodeUntilTimeoutNodeBasePtr) {
+  using ExecutorType = TypeParam;
+  ExecutorType executor;
+
+  auto start = std::chrono::steady_clock::now();
+  EXCEPT_NO_THROW(
+    rclcpp::executors::spin_node_until_timeout(
+      executor, this->node->get_node_base_interface(), 500ms));
+
+  // Make sure that timeout was reached
+  EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start);
+}
+
+// Check spin_node_until_timeout with node base pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinNodeUntilTimeoutNodeBasePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+    auto start = std::chrono::steady_clock::now();
+
+    EXCEPT_NO_THROW(
+      rclcpp::spin_until_future_complete(
+        node->get_node_base_interface(), shared_future, 500ms));
+
+    // Make sure that timeout was reached
+    EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start);
+  }
+
+  rclcpp::shutdown();
+}
+
+// Check spin_until_future_complete with node pointer (instantiates its own executor)
+TEST(TestExecutors, testSpinNodeUntilTimeoutNodePtr) {
+  rclcpp::init(0, nullptr);
+
+  {
+    auto node = std::make_shared<rclcpp::Node>("node");
+    auto start = std::chrono::steady_clock::now();
+
+    EXCEPT_NO_THROW(
+      rclcpp::spin_until_future_complete(
+        node->get_node_base_interface(), shared_future, 500ms));
+
+    // Make sure that timeout was reached
+    EXCEPT_GT(500ms, std::chrono::steady_clock::now() - start);
+  }
+
+  rclcpp::shutdown();
+}
+
 // Check spin_node_until_future_complete with node base pointer
 TYPED_TEST(TestExecutors, testSpinNodeUntilFutureCompleteNodeBasePtr) {
   using ExecutorType = TypeParam;