[sim] Add HAL hooks for simulationPeriodic (#2881)

This allows vendor libs to hook into the begin or end of simulationPeriodic().

hal/src/main/java/edu/wpi/first/hal/HAL.java

@@ -8,6 +8,8 @@
 package edu.wpi.first.hal;
 
 import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
 
 /**
  * JNI Wrapper for HAL<br>.
@@ -26,6 +28,40 @@ public final class HAL extends JNIWrapper {
 
   public static native void exitMain();
 
+  private static native void simPeriodicBeforeNative();
+
+  public static final List<Runnable> s_simPeriodicBefore = new ArrayList<>();
+
+  /**
+   * Runs SimPeriodicBefore callbacks.  IterativeRobotBase calls this prior
+   * to the user's simulationPeriodic code.
+   */
+  public static void simPeriodicBefore() {
+    simPeriodicBeforeNative();
+    synchronized (s_simPeriodicBefore) {
+      for (Runnable r : s_simPeriodicBefore) {
+        r.run();
+      }
+    }
+  }
+
+  private static native void simPeriodicAfterNative();
+
+  public static final List<Runnable> s_simPeriodicAfter = new ArrayList<>();
+
+  /**
+   * Runs SimPeriodicAfter callbacks.  IterativeRobotBase calls this after
+   * the user's simulationPeriodic code.
+   */
+  public static void simPeriodicAfter() {
+    simPeriodicAfterNative();
+    synchronized (s_simPeriodicAfter) {
+      for (Runnable r : s_simPeriodicAfter) {
+        r.run();
+      }
+    }
+  }
+
   public static native void observeUserProgramStarting();
 
   public static native void observeUserProgramDisabled();

hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java

@@ -7,6 +7,7 @@
 
 package edu.wpi.first.hal.simulation;
 
+import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.JNIWrapper;
 
 public class SimulatorJNI extends JNIWrapper {
@@ -21,4 +22,48 @@ public class SimulatorJNI extends JNIWrapper {
   public static native void stepTiming(long delta);
   public static native void stepTimingAsync(long delta);
   public static native void resetHandles();
+
+  public static class SimPeriodicBeforeCallback implements AutoCloseable {
+    private SimPeriodicBeforeCallback(Runnable r) {
+      m_run = r;
+    }
+
+    @Override
+    public void close() {
+      synchronized (HAL.s_simPeriodicBefore) {
+        HAL.s_simPeriodicBefore.remove(m_run);
+      }
+    }
+
+    private Runnable m_run;
+  }
+
+  public static SimPeriodicBeforeCallback registerSimPeriodicBeforeCallback(Runnable r) {
+    synchronized (HAL.s_simPeriodicBefore) {
+      HAL.s_simPeriodicBefore.add(r);
+    }
+    return new SimPeriodicBeforeCallback(r);
+  }
+
+  public static class SimPeriodicAfterCallback implements AutoCloseable {
+    private SimPeriodicAfterCallback(Runnable r) {
+      m_run = r;
+    }
+
+    @Override
+    public void close() {
+      synchronized (HAL.s_simPeriodicAfter) {
+        HAL.s_simPeriodicAfter.remove(m_run);
+      }
+    }
+
+    private Runnable m_run;
+  }
+
+  public static SimPeriodicAfterCallback registerSimPeriodicAfterCallback(Runnable r) {
+    synchronized (HAL.s_simPeriodicAfter) {
+      HAL.s_simPeriodicAfter.add(r);
+    }
+    return new SimPeriodicAfterCallback(r);
+  }
 }