[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);
+  }
 }

hal/src/main/native/athena/HAL.cpp

@@ -422,6 +422,10 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
 
 void HAL_Shutdown(void) {}
 
+void HAL_SimPeriodicBefore(void) {}
+
+void HAL_SimPeriodicAfter(void) {}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   if (feature == nullptr) {

hal/src/main/native/athena/mockdata/MockHooks.cpp

@@ -33,4 +33,18 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {}
 
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}
 
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {}
+
 }  // extern "C"

hal/src/main/native/cpp/jni/HAL.cpp

@@ -84,6 +84,30 @@ Java_edu_wpi_first_hal_HAL_exitMain
   HAL_ExitMain();
 }
 
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicBeforeNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicBeforeNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicBefore();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicAfterNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicAfter();
+}
+
 /*
  * Class:     edu_wpi_first_hal_HAL
  * Method:    observeUserProgramStarting

hal/src/main/native/include/hal/HALBase.h

@@ -157,6 +157,18 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
  */
 void HAL_Shutdown(void);
 
+/**
+ * Calls registered SimPeriodic "before" callbacks (only in simulation mode).
+ * This should be called prior to user code periodic simulation functions.
+ */
+void HAL_SimPeriodicBefore(void);
+
+/**
+ * Calls registered SimPeriodic "after" callbacks (only in simulation mode).
+ * This should be called after user code periodic simulation functions.
+ */
+void HAL_SimPeriodicAfter(void);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif

hal/src/main/native/include/hal/simulation/MockHooks.h

@@ -30,4 +30,13 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler);
 typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
 
+typedef void (*HALSIM_SimPeriodicCallback)(void* param);
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid);
+
 }  // extern "C"

hal/src/main/native/sim/HAL.cpp

@@ -26,13 +26,38 @@
 #include "hal/Extensions.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/simulation/DriverStationData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
 #include "mockdata/RoboRioDataInternal.h"
 
 using namespace hal;
 
+namespace {
+class SimPeriodicCallbackRegistry : public impl::SimCallbackRegistryBase {
+ public:
+  int32_t Register(HALSIM_SimPeriodicCallback callback, void* param) {
+    std::scoped_lock lock(m_mutex);
+    return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+  }
+
+  void operator()() const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
+    std::scoped_lock lock(m_mutex);
+#endif
+    if (m_callbacks) {
+      for (auto&& cb : *m_callbacks)
+        reinterpret_cast<HALSIM_SimPeriodicCallback>(cb.callback)(cb.param);
+    }
+  }
+};
+}  // namespace
+
 static HAL_RuntimeType runtimeType{HAL_Mock};
 static wpi::spinlock gOnShutdownMutex;
 static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown;
+static SimPeriodicCallbackRegistry gSimPeriodicBefore;
+static SimPeriodicCallbackRegistry gSimPeriodicAfter;
 
 namespace hal {
 namespace init {
@@ -333,6 +358,28 @@ void HAL_OnShutdown(void* param, void (*func)(void*)) {
   gOnShutdown.emplace_back(param, func);
 }
 
+void HAL_SimPeriodicBefore(void) { gSimPeriodicBefore(); }
+
+void HAL_SimPeriodicAfter(void) { gSimPeriodicAfter(); }
+
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicBefore.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {
+  gSimPeriodicBefore.Cancel(uid);
+}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicAfter.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {
+  gSimPeriodicAfter.Cancel(uid);
+}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   return 0;  // Do nothing for now

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -171,7 +171,9 @@ void IterativeRobotBase::LoopFunc() {
   m_watchdog.AddEpoch("Shuffleboard::Update()");
 
   if constexpr (IsSimulation()) {
+    HAL_SimPeriodicBefore();
     SimulationPeriodic();
+    HAL_SimPeriodicAfter();
     m_watchdog.AddEpoch("SimulationPeriodic()");
   }
 

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

@@ -292,7 +292,9 @@ public abstract class IterativeRobotBase extends RobotBase {
     m_watchdog.addEpoch("Shuffleboard.update()");
 
     if (isSimulation()) {
+      HAL.simPeriodicBefore();
       simulationPeriodic();
+      HAL.simPeriodicAfter();
       m_watchdog.addEpoch("simulationPeriodic()");
     }