Add an implementation of the spinAll method to the executors (#118)

* Add an implementation of spinAll().

This differs from spinSome() in how it looks at ready
executables.  spinSome() collects a list of ready
executables once at the beginning of execution, and then
executes each one until a timeout.  spinAll() will continually
collect new executables, executing each one until a timeout.

This is basically a direct port from the rclcpp version of
the method with the same name.

Signed-off-by: Chris Lalancette <[email protected]>

* Add in SpinTest.java.

Signed-off-by: Chris Lalancette <[email protected]>

* Remove rogue comment.

Signed-off-by: Chris Lalancette <[email protected]>

Author: clalancette

rcljava/CMakeLists.txt

@@ -221,6 +221,7 @@ if(BUILD_TESTING)
 
   set(${PROJECT_NAME}_test_sources
     "src/test/java/org/ros2/rcljava/RCLJavaTest.java"
+    "src/test/java/org/ros2/rcljava/SpinTest.java"
     "src/test/java/org/ros2/rcljava/TimeTest.java"
     "src/test/java/org/ros2/rcljava/client/ClientTest.java"
     "src/test/java/org/ros2/rcljava/node/NodeTest.java"
@@ -233,6 +234,7 @@ if(BUILD_TESTING)
 
   set(${PROJECT_NAME}_testsuites
     "org.ros2.rcljava.RCLJavaTest"
+    "org.ros2.rcljava.SpinTest"
     "org.ros2.rcljava.TimeTest"
     "org.ros2.rcljava.client.ClientTest"
     "org.ros2.rcljava.node.NodeTest"

rcljava/src/main/java/org/ros2/rcljava/executors/BaseExecutor.java

@@ -328,19 +328,46 @@ protected AnyExecutable getNextExecutable() {
     return null;
   }
 
-  protected void spinSome() {
-    AnyExecutable anyExecutable = getNextExecutable();
-    if (anyExecutable == null) {
-      waitForWork(0);
-      anyExecutable = getNextExecutable();
+  private boolean maxDurationNotElapsed(long maxDurationNs, long startNs) {
+    long nowNs = System.nanoTime();
+    if (maxDurationNs == 0) {
+      // told to spin forever if need be
+      return true;
+    } else if (nowNs - startNs < maxDurationNs) {
+      // told to spin only for some maximum amount of time
+      return true;
     }
+    // spun too long
+    return false;
+  }
 
-    while (anyExecutable != null) {
-      executeAnyExecutable(anyExecutable);
-      anyExecutable = getNextExecutable();
+  private void spinSomeImpl(long maxDurationNs, boolean exhaustive) {
+    long startNs = System.nanoTime();
+    boolean workAvailable = false;
+    while (RCLJava.ok() && maxDurationNotElapsed(maxDurationNs, startNs)) {
+      if (!workAvailable) {
+        waitForWork(0);
+      }
+      AnyExecutable anyExecutable = getNextExecutable();
+      if (anyExecutable != null) {
+        executeAnyExecutable(anyExecutable);
+      } else {
+        if (!workAvailable || !exhaustive) {
+          break;
+        }
+        workAvailable = false;
+      }
     }
   }
 
+  protected void spinSome(long maxDurationNs) {
+    spinSomeImpl(maxDurationNs, false);
+  }
+
+  protected void spinAll(long maxDurationNs) {
+    spinSomeImpl(maxDurationNs, true);
+  }
+
   protected void spinOnce(long timeout) {
     AnyExecutable anyExecutable = getNextExecutable();
     if (anyExecutable == null) {

rcljava/src/main/java/org/ros2/rcljava/executors/Executor.java

@@ -18,15 +18,19 @@
 import org.ros2.rcljava.node.ComposableNode;
 
 public interface Executor {
-  public void spin();
+  public void addNode(ComposableNode node);
+
+  public void removeNode(ComposableNode node);
 
   public void spinOnce();
 
   public void spinOnce(long timeout);
 
   public void spinSome();
 
-  public void addNode(ComposableNode node);
+  public void spinSome(long maxDurationNs);
 
-  public void removeNode(ComposableNode node);
+  public void spinAll(long maxDurationNs);
+
+  public void spin();
 }

rcljava/src/main/java/org/ros2/rcljava/executors/MultiThreadedExecutor.java

@@ -56,7 +56,15 @@ public void spinOnce(long timeout) {
   }
 
   public void spinSome() {
-    this.baseExecutor.spinSome();
+    this.spinSome(0);
+  }
+
+  public void spinSome(long maxDurationNs) {
+    this.baseExecutor.spinSome(maxDurationNs);
+  }
+
+  public void spinAll(long maxDurationNs) {
+    this.baseExecutor.spinAll(maxDurationNs);
   }
 
   public void spin() {

rcljava/src/main/java/org/ros2/rcljava/executors/SingleThreadedExecutor.java

@@ -39,7 +39,15 @@ public void spinOnce(long timeout) {
   }
 
   public void spinSome() {
-    this.baseExecutor.spinSome();
+    this.spinSome(0);
+  }
+
+  public void spinSome(long maxDurationNs) {
+    this.baseExecutor.spinSome(maxDurationNs);
+  }
+
+  public void spinAll(long maxDurationNs) {
+    this.baseExecutor.spinAll(maxDurationNs);
   }
 
   public void spin() {