From c4b0a588f5b0f98470dda824e191c5f4aad2048a Mon Sep 17 00:00:00 2001
From: Jacob Perron <jacob@openrobotics.org>
Date: Fri, 9 Jul 2021 17:13:32 -0700
Subject: [PATCH] Avoid blocking forever in RCLFuture.get() with timeout

Add overloads for spinOnce methods to include a timeout argument and call this when spinning in the future.

This fixes a bug where the future.get(...) call blocks forever, e.g. when waiting on a service response and the service becomes unavailable.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
---
 .../main/java/org/ros2/rcljava/RCLJava.java   | 47 +++++++++++++++++--
 .../ros2/rcljava/concurrent/RCLFuture.java    |  4 +-
 2 files changed, 45 insertions(+), 6 deletions(-)

diff --git a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
index 6c820ed9..92c44f6f 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/RCLJava.java
@@ -287,22 +287,61 @@ public static void spin(final ComposableNode composableNode) {
     getGlobalExecutor().removeNode(composableNode);
   }
 
-  public static void spinOnce(final Node node) {
+  /**
+   * Execute one callback for a @{link Node}.
+   *
+   * Wait until a callback becomes ready and then execute it.
+   *
+   * @param node The node to spin on.
+   * @param timeout The time to wait for a callback to become ready in nanoseconds.
+   *   If a timeout occurs, then nothing happens and this method returns.
+   */
+  public static void spinOnce(final Node node, long timeout) {
     ComposableNode composableNode = new ComposableNode() {
       public Node getNode() {
         return node;
       }
     };
     getGlobalExecutor().addNode(composableNode);
-    getGlobalExecutor().spinOnce();
+    getGlobalExecutor().spinOnce(timeout);
     getGlobalExecutor().removeNode(composableNode);
   }
 
-  public static void spinOnce(final ComposableNode composableNode) {
+  /**
+   * Execute one callback for a @{link Node}.
+   *
+   * Wait until a callback becomes ready and then execute it.
+   *
+   * @param node The node to spin on.
+   */
+  public static void spinOnce(final Node node) {
+    RCLJava.spinOnce(node, -1);
+  }
+
+  /**
+   * Execute one callback for a @{link ComposableNode}.
+   *
+   * Wait until a callback becomes ready and then execute it.
+   *
+   * @param composableNode The composable node to spin on.
+   * @param timeout The time to wait for a callback to become ready in nanoseconds.
+   *   If a timeout occurs, then nothing happens and this method returns.
+   */
+  public static void spinOnce(final ComposableNode composableNode, long timeout) {
     getGlobalExecutor().addNode(composableNode);
-    getGlobalExecutor().spinOnce();
+    getGlobalExecutor().spinOnce(timeout);
     getGlobalExecutor().removeNode(composableNode);
   }
+  /**
+   * Execute one callback for a @{link ComposableNode}.
+   *
+   * Wait until a callback becomes ready and then execute it.
+   *
+   * @param composableNode The composable node to spin on.
+   */
+  public static void spinOnce(final ComposableNode composableNode) {
+    RCLJava.spinOnce(composableNode, -1);
+  }
 
   public static void spinSome(final Node node) {
     ComposableNode composableNode = new ComposableNode() {
diff --git a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java
index 1ac88b55..5b45ccc4 100644
--- a/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java
+++ b/rcljava/src/main/java/org/ros2/rcljava/concurrent/RCLFuture.java
@@ -73,13 +73,13 @@ public final V get(final long timeout, final TimeUnit unit)
 
     while (RCLJava.ok()) {
       if (executor != null) {
-        executor.spinOnce();
+        executor.spinOnce(timeoutNS);
       } else {
         Node node = nodeReference.get();
         if (node == null) {
           return null; // TODO(esteve) do something
         }
-        RCLJava.spinOnce(node);
+        RCLJava.spinOnce(node, timeoutNS);
       }
 
       if (isDone()) {