Add ability to end startCompetition() main loop (#2032)

This is useful for both cleanly exiting from simulation and for unit testing
at a framework level.

This change required removing move constructor/assignment from IterativeRobot.

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -1008,6 +1008,16 @@ public class DriverStation {
     m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
   }
 
+  /**
+   * Forces waitForData() to return immediately.
+   */
+  public void wakeupWaitForData() {
+    m_waitForDataMutex.lock();
+    m_waitForDataCount++;
+    m_waitForDataCond.signalAll();
+    m_waitForDataMutex.unlock();
+  }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
@@ -1061,11 +1071,7 @@ public class DriverStation {
       m_cacheDataMutex.unlock();
     }
 
-    m_waitForDataMutex.lock();
-    m_waitForDataCount++;
-    m_waitForDataCond.signalAll();
-    m_waitForDataMutex.unlock();
-
+    wakeupWaitForData();
     sendMatchData();
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java

@@ -25,6 +25,7 @@ import edu.wpi.first.hal.HAL;
 @Deprecated
 public class IterativeRobot extends IterativeRobotBase {
   private static final double kPacketPeriod = 0.02;
+  private volatile boolean m_exit;
 
   /**
    * Create a new IterativeRobot.
@@ -49,8 +50,20 @@ public class IterativeRobot extends IterativeRobotBase {
     while (!Thread.currentThread().isInterrupted()) {
       // Wait for new data to arrive
       m_ds.waitForData();
+      if (m_exit) {
+        break;
+      }
 
       loopFunc();
     }
   }
+
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  @Override
+  public void endCompetition() {
+    m_exit = true;
+    m_ds.wakeupWaitForData();
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -12,6 +12,7 @@ import java.io.IOException;
 import java.io.OutputStream;
 import java.nio.charset.StandardCharsets;
 import java.nio.file.Files;
+import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Supplier;
 
 import edu.wpi.cscore.CameraServerJNI;
@@ -189,6 +190,11 @@ public abstract class RobotBase implements AutoCloseable {
    */
   public abstract void startCompetition();
 
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  public abstract void endCompetition();
+
   @SuppressWarnings("JavadocMethod")
   public static boolean getBooleanProperty(String name, boolean defaultValue) {
     String propVal = System.getProperty(name);
@@ -204,6 +210,10 @@ public abstract class RobotBase implements AutoCloseable {
     }
   }
 
+  private static final ReentrantLock m_runMutex = new ReentrantLock();
+  private static RobotBase m_robotCopy;
+  private static boolean m_suppressExitWarning;
+
   /**
    * Run the robot main loop.
    */
@@ -232,6 +242,10 @@ public abstract class RobotBase implements AutoCloseable {
       return;
     }
 
+    m_runMutex.lock();
+    m_robotCopy = robot;
+    m_runMutex.unlock();
+
     if (isReal()) {
       try {
         final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
@@ -265,18 +279,32 @@ public abstract class RobotBase implements AutoCloseable {
           throwable.getStackTrace());
       errorOnExit = true;
     } finally {
-      // startCompetition never returns unless exception occurs....
-      DriverStation.reportWarning("Robots should not quit, but yours did!", false);
-      if (errorOnExit) {
-        DriverStation.reportError(
-            "The startCompetition() method (or methods called by it) should have "
-                + "handled the exception above.", false);
-      } else {
-        DriverStation.reportError("Unexpected return from startCompetition() method.", false);
+      m_runMutex.lock();
+      boolean suppressExitWarning = m_suppressExitWarning;
+      m_runMutex.unlock();
+      if (!suppressExitWarning) {
+        // startCompetition never returns unless exception occurs....
+        DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+        if (errorOnExit) {
+          DriverStation.reportError(
+              "The startCompetition() method (or methods called by it) should have "
+                  + "handled the exception above.", false);
+        } else {
+          DriverStation.reportError("Unexpected return from startCompetition() method.", false);
+        }
       }
     }
   }
 
+  /**
+   * Suppress the "Robots should not quit" message.
+   */
+  public static void suppressExitWarning(boolean value) {
+    m_runMutex.lock();
+    m_suppressExitWarning = value;
+    m_runMutex.unlock();
+  }
+
   /**
    * Starting point for the applications.
    */
@@ -299,6 +327,18 @@ public abstract class RobotBase implements AutoCloseable {
       thread.setDaemon(true);
       thread.start();
       HAL.runMain();
+      suppressExitWarning(true);
+      m_runMutex.lock();
+      RobotBase robot = m_robotCopy;
+      m_runMutex.unlock();
+      if (robot != null) {
+        robot.endCompetition();
+      }
+      try {
+        thread.join(1000);
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
     } else {
       runRobot(robotSupplier);
     }

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -82,6 +82,14 @@ public class TimedRobot extends IterativeRobotBase {
     }
   }
 
+  /**
+   * Ends the main loop in startCompetition().
+   */
+  @Override
+  public void endCompetition() {
+    NotifierJNI.stopNotifier(m_notifier);
+  }
+
   /**
    * Get time period between calls to Periodic() functions.
    */