[hal, wpilib] Add OpMode support (#7744)

User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp

@@ -5,8 +5,10 @@
 #pragma once
 
 #include <functional>
+#include <span>
 #include <string_view>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/Value.h"
 
 namespace wpi::sim {
@@ -14,6 +16,8 @@ namespace wpi::sim {
 using NotifyCallback = std::function<void(std::string_view, const HAL_Value*)>;
 using ConstBufferCallback = std::function<void(
     std::string_view, const unsigned char* buffer, unsigned int count)>;
+using OpModeOptionsCallback =
+    std::function<void(std::string_view, std::span<const HAL_OpModeOption>)>;
 using CancelCallbackFunc = void (*)(int32_t index, int32_t uid);
 using CancelCallbackNoIndexFunc = void (*)(int32_t uid);
 using CancelCallbackChannelFunc = void (*)(int32_t index, int32_t channel,
@@ -23,6 +27,9 @@ void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
 void ConstBufferCallbackStoreThunk(const char* name, void* param,
                                    const unsigned char* buffer,
                                    unsigned int count);
+void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
+                                     const HAL_OpModeOption* opmodes,
+                                     int32_t count);
 
 /**
  * Manages simulation callbacks; each object is associated with a callback.
@@ -46,6 +53,9 @@ class CallbackStore {
   CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
                 CancelCallbackChannelFunc ccf);
 
+  CallbackStore(int32_t u, OpModeOptionsCallback cb,
+                CancelCallbackNoIndexFunc ccf);
+
   CallbackStore(const CallbackStore&) = delete;
   CallbackStore& operator=(const CallbackStore&) = delete;
 
@@ -60,6 +70,10 @@ class CallbackStore {
                                             const unsigned char* buffer,
                                             unsigned int count);
 
+  friend void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
+                                              const HAL_OpModeOption* opmodes,
+                                              int32_t count);
+
  private:
   int32_t index;
   int32_t channel;
@@ -67,6 +81,7 @@ class CallbackStore {
 
   NotifyCallback callback;
   ConstBufferCallback constBufferCallback;
+  OpModeOptionsCallback opModeOptionsCallback;
   union {
     CancelCallbackFunc ccf;
     CancelCallbackChannelFunc cccf;

wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp

@@ -4,14 +4,46 @@
 
 #pragma once
 
+#include <stdint.h>
+
 #include <memory>
 
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStationTypes.h"
+#include "wpi/hal/simulation/DriverStationData.h"
 #include "wpi/simulation/CallbackStore.hpp"
 
 namespace wpi::sim {
 
+class OpModeOptions : public std::span<HAL_OpModeOption> {
+ public:
+  OpModeOptions() = default;
+  OpModeOptions(HAL_OpModeOption* options, int32_t len)
+      : span{options, options + len} {}
+  OpModeOptions(const OpModeOptions&) = delete;
+
+  OpModeOptions(OpModeOptions&& oth) : span{oth} {
+    static_cast<span&>(oth) = {};
+  }
+
+  OpModeOptions& operator=(const OpModeOptions&) = delete;
+
+  OpModeOptions& operator=(OpModeOptions&& oth) {
+    if (data()) {
+      HALSIM_FreeOpModeOptionsArray(data(), size());
+    }
+    static_cast<span&>(*this) = oth;
+    static_cast<span&>(oth) = {};
+    return *this;
+  }
+
+  ~OpModeOptions() {
+    if (data()) {
+      HALSIM_FreeOpModeOptionsArray(data(), size());
+    }
+  }
+};
+
 /**
  * Class to control a simulated driver station.
  */
@@ -44,56 +76,29 @@ class DriverStationSim {
   static void SetEnabled(bool enabled);
 
   /**
-   * Register a callback on whether the DS is in autonomous mode.
+   * Register a callback on DS robot mode changes.
    *
-   * @param callback the callback that will be called on autonomous mode
-   *                 entrance/exit
+   * @param callback the callback that will be called when robot mode changes
    * @param initialNotify if true, the callback will be run on the initial value
    * @return the CallbackStore object associated with this callback
    */
   [[nodiscard]]
-  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+  static std::unique_ptr<CallbackStore> RegisterRobotModeCallback(
       NotifyCallback callback, bool initialNotify);
 
   /**
-   * Check if the DS is in autonomous.
+   * Get the robot mode set by the DS.
    *
-   * @return true if autonomous
+   * @return robot mode
    */
-  static bool GetAutonomous();
+  static HAL_RobotMode GetRobotMode();
 
   /**
-   * Change whether the DS is in autonomous.
+   * Change the robot mode set by the DS.
    *
-   * @param autonomous the new value
+   * @param robotMode the new value
    */
-  static void SetAutonomous(bool autonomous);
-
-  /**
-   * Register a callback on whether the DS is in test mode.
-   *
-   * @param callback the callback that will be called whenever the test mode
-   *                 is entered or left
-   * @param initialNotify if true, the callback will be run on the initial value
-   * @return the CallbackStore object associated with this callback
-   */
-  [[nodiscard]]
-  static std::unique_ptr<CallbackStore> RegisterTestCallback(
-      NotifyCallback callback, bool initialNotify);
-
-  /**
-   * Check if the DS is in test.
-   *
-   * @return true if test
-   */
-  static bool GetTest();
-
-  /**
-   * Change whether the DS is in test.
-   *
-   * @param test the new value
-   */
-  static void SetTest(bool test);
+  static void SetRobotMode(HAL_RobotMode robotMode);
 
   /**
    * Register a callback on the eStop state.
@@ -225,6 +230,50 @@ class DriverStationSim {
    */
   static void SetMatchTime(double matchTime);
 
+  /**
+   * Register a callback on DS opmode changes.
+   *
+   * @param callback the callback that will be called when opmode changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterOpModeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the opmode set by the DS.
+   *
+   * @return opmode
+   */
+  static int64_t GetOpMode();
+
+  /**
+   * Change the opmode set by the DS.
+   *
+   * @param opmode the new value
+   */
+  static void SetOpMode(int64_t opmode);
+
+  /**
+   * Register a callback on opmode options changes.
+   *
+   * @param callback the callback that will be called when the list of opmodes
+   * changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback.
+   */
+  [[nodiscard]]
+  static std::unique_ptr<CallbackStore> RegisterOpModeOptionsCallback(
+      OpModeOptionsCallback callback, bool initialNotify);
+
+  /**
+   * Gets the list of opmode options.
+   *
+   * @return opmodes list
+   */
+  static OpModeOptions GetOpModeOptions();
+
   /**
    * Updates DriverStation data so that new values are visible to the user
    * program.

wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp

@@ -6,6 +6,7 @@
 
 #include <stdint.h>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/HALBase.h"
 #include "wpi/units/time.hpp"
 
@@ -37,6 +38,20 @@ void SetProgramStarted(bool started);
  */
 bool GetProgramStarted();
 
+/**
+ * Sets the user program state (control word).
+ *
+ * @param controlWord control word
+ */
+void SetProgramState(wpi::hal::ControlWord controlWord);
+
+/**
+ * Gets the user program state (control word).
+ *
+ * @return Control word
+ */
+wpi::hal::ControlWord GetProgramState();
+
 /**
  * Restart the simulator time.
  */