Add support for spin_until_complete (take 2)

rclcpp/CMakeLists.txt

@@ -43,6 +43,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/callback_group.cpp
   src/rclcpp/client.cpp
   src/rclcpp/clock.cpp
+  src/rclcpp/condition_wait_return_code.cpp
   src/rclcpp/context.cpp
   src/rclcpp/contexts/default_context.cpp
   src/rclcpp/create_generic_client.cpp
@@ -73,7 +74,6 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/expand_topic_or_service_name.cpp
   src/rclcpp/experimental/executors/events_executor/events_executor.cpp
   src/rclcpp/experimental/timers_manager.cpp
-  src/rclcpp/future_return_code.cpp
   src/rclcpp/generic_client.cpp
   src/rclcpp/generic_publisher.cpp
   src/rclcpp/generic_subscription.cpp

rclcpp/include/rclcpp/condition_wait_return_code.hpp

@@ -0,0 +1,47 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_
+#define RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_
+
+#include <iostream>
+#include <string>
+
+#include "rclcpp/visibility_control.hpp"
+
+namespace rclcpp
+{
+
+/// Return codes to be used with spin_until_complete.
+/**
+ * SUCCESS: The condition wait is complete. This does not indicate that the operation succeeded.
+ * INTERRUPTED: The condition wait is not complete, spinning was interrupted by Ctrl-C or another
+ *              error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum class ConditionWaitReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+/// Stream operator for ConditionWaitReturnCode.
+RCLCPP_PUBLIC
+std::ostream &
+operator<<(std::ostream & os, const ConditionWaitReturnCode & condition_wait_return_code);
+
+/// String conversion function for ConditionWaitReturnCode.
+RCLCPP_PUBLIC
+std::string
+to_string(const ConditionWaitReturnCode & condition_wait_return_code);
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__CONDITION_WAIT_RETURN_CODE_HPP_

rclcpp/include/rclcpp/executor.hpp

@@ -32,6 +32,7 @@
 #include "rclcpp/executors/executor_notify_waitable.hpp"
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/condition_wait_return_code.hpp"
 #include "rclcpp/context.hpp"
 #include "rclcpp/contexts/default_context.hpp"
 #include "rclcpp/guard_condition.hpp"
@@ -350,32 +351,23 @@ 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. If this condition is not related to
+   *   what the executor is waiting on and the timeout is infinite, this could block forever.
    * \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,
+  template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+  ConditionWaitReturnCode
+  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,18 +376,21 @@ class Executor
     }
     std::chrono::nanoseconds timeout_left = timeout_ns;
 
+    // Preliminary check, finish if condition is done already.
+    if (condition()) {
+      return ConditionWaitReturnCode::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) {
-        return FutureReturnCode::SUCCESS;
+      if (condition()) {
+        return ConditionWaitReturnCode::SUCCESS;
       }
       // If the original timeout is < 0, then this is blocking, never TIMEOUT.
       if (timeout_ns < std::chrono::nanoseconds::zero()) {
@@ -404,14 +399,47 @@ class Executor
       // 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;
+        return ConditionWaitReturnCode::TIMEOUT;
       }
       // Subtract the elapsed time from the original timeout.
       timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
     }
 
-    // The future did not complete before ok() returned false, return INTERRUPTED.
-    return FutureReturnCode::INTERRUPTED;
+    // The condition did not pass before ok() returned false, return INTERRUPTED.
+    return ConditionWaitReturnCode::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> 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.

rclcpp/include/rclcpp/executors.hpp

@@ -18,10 +18,12 @@
 #include <future>
 #include <memory>
 
+#include "rclcpp/condition_wait_return_code.hpp"
 #include "rclcpp/executors/multi_threaded_executor.hpp"
 #include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp/executors/static_single_threaded_executor.hpp"
 #include "rclcpp/experimental/executors/events_executor/events_executor.hpp"
+#include "rclcpp/future_return_code.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -67,6 +69,49 @@ 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. If this condition is not related to
+ *   what the executor is waiting on and the timeout is infinite, this could block forever.
+ * \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::ConditionWaitReturnCode
+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::ConditionWaitReturnCode
+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 +158,27 @@ spin_node_until_future_complete(
 
 }  // namespace executors
 
+template<typename TimeRepT = int64_t, typename TimeT = std::milli>
+rclcpp::ConditionWaitReturnCode
+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::ConditionWaitReturnCode
+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/future_return_code.hpp

@@ -18,6 +18,7 @@
 #include <iostream>
 #include <string>
 
+#include "rclcpp/condition_wait_return_code.hpp"
 #include "rclcpp/visibility_control.hpp"
 
 namespace rclcpp
@@ -30,17 +31,7 @@ namespace rclcpp
  * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
  * TIMEOUT: Spinning timed out.
  */
-enum class FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
-
-/// Stream operator for FutureReturnCode.
-RCLCPP_PUBLIC
-std::ostream &
-operator<<(std::ostream & os, const FutureReturnCode & future_return_code);
-
-/// String conversion function for FutureReturnCode.
-RCLCPP_PUBLIC
-std::string
-to_string(const FutureReturnCode & future_return_code);
+using FutureReturnCode = ConditionWaitReturnCode;
 
 }  // namespace rclcpp
 

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/src/rclcpp/future_return_code.cpp → rclcpp/src/rclcpp/condition_wait_return_code.cpp

@@ -16,31 +16,31 @@
 #include <string>
 #include <type_traits>
 
-#include "rclcpp/future_return_code.hpp"
+#include "rclcpp/condition_wait_return_code.hpp"
 
 namespace rclcpp
 {
 
 std::ostream &
-operator<<(std::ostream & os, const rclcpp::FutureReturnCode & future_return_code)
+operator<<(std::ostream & os, const rclcpp::ConditionWaitReturnCode & condition_wait_return_code)
 {
-  return os << to_string(future_return_code);
+  return os << to_string(condition_wait_return_code);
 }
 
 std::string
-to_string(const rclcpp::FutureReturnCode & future_return_code)
+to_string(const rclcpp::ConditionWaitReturnCode & condition_wait_return_code)
 {
-  using enum_type = std::underlying_type<FutureReturnCode>::type;
+  using enum_type = std::underlying_type<ConditionWaitReturnCode>::type;
   std::string prefix = "Unknown enum value (";
-  std::string ret_as_string = std::to_string(static_cast<enum_type>(future_return_code));
-  switch (future_return_code) {
-    case FutureReturnCode::SUCCESS:
+  std::string ret_as_string = std::to_string(static_cast<enum_type>(condition_wait_return_code));
+  switch (condition_wait_return_code) {
+    case ConditionWaitReturnCode::SUCCESS:
       prefix = "SUCCESS (";
       break;
-    case FutureReturnCode::INTERRUPTED:
+    case ConditionWaitReturnCode::INTERRUPTED:
       prefix = "INTERRUPTED (";
       break;
-    case FutureReturnCode::TIMEOUT:
+    case ConditionWaitReturnCode::TIMEOUT:
       prefix = "TIMEOUT (";
       break;
   }

rclcpp/test/rclcpp/CMakeLists.txt

@@ -109,6 +109,12 @@ ament_add_gtest(test_function_traits test_function_traits.cpp)
 if(TARGET test_function_traits)
   target_link_libraries(test_function_traits ${PROJECT_NAME})
 endif()
+ament_add_gtest(
+  test_condition_wait_return_code
+  test_condition_wait_return_code.cpp)
+if(TARGET test_condition_wait_return_code)
+  target_link_libraries(test_condition_wait_return_code ${PROJECT_NAME})
+endif()
 ament_add_gtest(
   test_future_return_code
   test_future_return_code.cpp)

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