From b860b899e57e168d1265c03eb624f320bc664d8f Mon Sep 17 00:00:00 2001
From: Shane Loretz <shane.loretz@gmail.com>
Date: Mon, 17 Sep 2018 15:51:15 -0700
Subject: [PATCH] Add max_duration to spin_some() (#558)

With max_duration spin_some will execute work until it has spent more
time than the elapsed duration.
---
 rclcpp/include/rclcpp/executor.hpp |  6 +++++-
 rclcpp/src/rclcpp/executor.cpp     | 17 +++++++++++++++--
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index 657529f..baff9f1 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -190,10 +190,14 @@ public:
    * single-threaded model of execution.
    * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function
    * to block (which may have unintended consequences).
+   *
+   * \param[in] max_duration The maximum amount of time to spend executing work, or 0 for no limit.
+   * Note that spin_some() may take longer than this time as it only returns once max_duration has
+   * been exceeded.
    */
   RCLCPP_PUBLIC
   virtual void
-  spin_some();
+  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0));
 
   RCLCPP_PUBLIC
   virtual void
diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index 308bd55..cd93ca5 100644
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -200,13 +200,26 @@ Executor::spin_node_some(std::shared_ptr<rclcpp::Node> node)
 }
 
 void
-Executor::spin_some()
+Executor::spin_some(std::chrono::nanoseconds max_duration)
 {
+  auto start = std::chrono::steady_clock::now();
+  auto max_duration_not_elapsed = [max_duration, start]() {
+      if (std::chrono::nanoseconds(0) == max_duration) {
+        // told to spin forever if need be
+        return true;
+      } else if (std::chrono::steady_clock::now() - start < max_duration) {
+        // told to spin only for some maximum amount of time
+        return true;
+      }
+      // spun too long
+      return false;
+    };
+
   if (spinning.exchange(true)) {
     throw std::runtime_error("spin_some() called while already spinning");
   }
   RCLCPP_SCOPE_EXIT(this->spinning.store(false); );
-  while (spinning.load()) {
+  while (spinning.load() && max_duration_not_elapsed()) {
     AnyExecutable any_exec;
     if (get_next_executable(any_exec, std::chrono::milliseconds::zero())) {
       execute_any_executable(any_exec);