[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/opmode/LinearOpMode.hpp

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+
+#include "wpi/opmode/OpMode.hpp"
+
+namespace wpi {
+
+/**
+ * An opmode structure for "linear" operation. The user is responsible for
+ * implementing any looping behavior; after Run() returns it will not be called
+ * again on the same object.
+ *
+ * Lifecycle:
+ *
+ * - Constructed when opmode selected on driver station
+ *
+ * - DisabledPeriodic() called periodically as long as DS is disabled
+ *
+ * - When DS transitions from disabled to enabled, Run() is called exactly once
+ *
+ * - When DS transitions from enabled to disabled, or a different opmode is
+ *   selected on the driver station, object is destroyed and not reused
+ *
+ * The user is responsible for exiting Run() when the opmode is directed to stop
+ * executing. This is indicated by IsRunning() returning false. All other
+ * methods should be written to return as quickly as possible when IsRunning()
+ * returns false.
+ */
+class LinearOpMode : public OpMode {
+ public:
+  /**
+   * Called periodically while the opmode is selected on the DS and the robot is
+   * disabled.
+   */
+  void DisabledPeriodic() override {}
+
+  /**
+   * Called once when the robot is enabled. When it returns, it will not be
+   * called again on the same object.
+   */
+  virtual void Run() = 0;
+
+  /**
+   * Returns true while this opmode is selected (regardless of enable state).
+   * All other functions should be written to return as quickly as possible when
+   * this returns false.
+   *
+   * @return True if opmode selected, false otherwise
+   */
+  bool IsRunning() const { return m_running; }
+
+  // implements OpMode interface
+  void OpModeRun(int64_t opModeId) final;
+
+  void OpModeStop() final;
+
+ private:
+  std::atomic_bool m_running{true};
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+namespace wpi {
+
+/**
+ * Top-level interface for opmode classes. Users should generally extend one of
+ * the abstract implementations of this interface (e.g. PeriodicOpMode or
+ * LinearOpMode) rather than directly implementing this interface.
+ */
+class OpMode {
+ public:
+  /**
+   * The object is destroyed when the opmode is no longer selected on the DS or
+   * after OpModeRun() returns.
+   */
+  virtual ~OpMode() = default;
+
+  /**
+   * This function is called periodically while the opmode is selected on the DS
+   * (robot is disabled). Code that should only run once when the opmode is
+   * selected should go in the opmode constructor.
+   */
+  virtual void DisabledPeriodic() {}
+
+  /**
+   * This function is called when the opmode starts (robot is enabled).
+   *
+   * @param opModeId opmode unique ID
+   */
+  virtual void OpModeRun(int64_t opModeId) = 0;
+
+  /**
+   * This function is called asynchronously when the robot is disabled, to
+   * request the opmode return from OpModeRun().
+   */
+  virtual void OpModeStop() = 0;
+};
+
+}  // namespace wpi

wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp

@@ -0,0 +1,178 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <chrono>
+#include <functional>
+#include <vector>
+
+#include "wpi/hal/Notifier.h"
+#include "wpi/hal/Types.h"
+#include "wpi/opmode/OpMode.hpp"
+#include "wpi/system/Watchdog.hpp"
+#include "wpi/units/time.hpp"
+#include "wpi/util/priority_queue.hpp"
+
+namespace wpi {
+
+/**
+ * An opmode structure for periodic operation. This base class implements a loop
+ * that runs one or more functions periodically (on a set time interval aka loop
+ * period). The primary periodic callback function is the Periodic() function;
+ * the time interval for this callback is 20 ms by default, but may be changed
+ * via passing a different time interval to the constructor. Additional periodic
+ * callbacks with different intervals can be added using the AddPeriodic() set
+ * of functions.
+ *
+ * Lifecycle:
+ *
+ * - Constructed when opmode selected on driver station
+ *
+ * - DisabledPeriodic() called periodically as long as DS is disabled. Note
+ *   this is not called on a set time interval (it does not use the same time
+ *   interval as Periodic())
+ *
+ * - When DS transitions from disabled to enabled, Start() is called once
+ *
+ * - While DS is enabled, Periodic() is called periodically on the time interval
+ *   set by the constructor, and additional periodic callbacks added via
+ *   AddPeriodic() are called periodically on their set time intervals
+ *
+ * - When DS transitions from enabled to disabled, or a different opmode is
+ *   selected on the driver station when the DS is enabled, End() is called,
+ *   followed by the object being destroyed; the object is not reused
+ *
+ * - If a different opmode is selected on the driver station when the DS is
+ *   disabled, the object is destroyed (without End() being called); the object
+ *   is not reused
+ */
+class PeriodicOpMode : public OpMode {
+ public:
+  /** Default loop period. */
+  static constexpr auto kDefaultPeriod = 20_ms;
+
+ protected:
+  /**
+   * Constructor. Periodic opmodes may specify the period used for the
+   * Periodic() function.
+   *
+   * @param period period for callbacks to the Periodic() function
+   */
+  explicit PeriodicOpMode(wpi::units::second_t period = kDefaultPeriod);
+
+ public:
+  ~PeriodicOpMode() override;
+
+  /**
+   * Called periodically while the opmode is selected on the DS (robot is
+   * disabled).
+   */
+  void DisabledPeriodic() override {}
+
+  /**
+   * Called a single time when the robot transitions from disabled to enabled.
+   * This is called prior to Periodic() being called.
+   */
+  virtual void Start() {}
+
+  /** Called periodically while the robot is enabled. */
+  virtual void Periodic() = 0;
+
+  /**
+   * Called a single time when the robot transitions from enabled to disabled,
+   * or just before the destructor is called if a different opmode is selected
+   * while the robot is enabled.
+   */
+  virtual void End() {}
+
+  /**
+   * Return the system clock time in microseconds for the start of the current
+   * periodic loop. This is in the same time base as Timer.getFPGATimestamp(),
+   * but is stable through a loop. It is updated at the beginning of every
+   * periodic callback (including the normal periodic loop).
+   *
+   * @return Robot running time in microseconds, as of the start of the current
+   * periodic function.
+   */
+  int64_t GetLoopStartTime() const { return m_loopStartTimeUs; }
+
+  /**
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * This is scheduled on the same Notifier as Periodic(), so Periodic() and the
+   * callback run synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period   The period at which to run the callback.
+   * @param offset   The offset from the common starting time. This is useful
+   *                 for scheduling a callback in a different timeslot relative
+   *                 to TimedRobot.
+   */
+  void AddPeriodic(std::function<void()> callback, wpi::units::second_t period,
+                   wpi::units::second_t offset = 0_s);
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   */
+  wpi::units::second_t GetPeriod() const { return m_period; }
+
+  /**
+   * Prints list of epochs added so far and their times.
+   */
+  void PrintWatchdogEpochs();
+
+ protected:
+  /** Loop function. */
+  void LoopFunc();
+
+ public:
+  // implements OpMode interface
+  void OpModeRun(int64_t opModeId) final;
+
+  void OpModeStop() final;
+
+ private:
+  class Callback {
+   public:
+    std::function<void()> func;
+    std::chrono::microseconds period;
+    std::chrono::microseconds expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func      The callback to run.
+     * @param startTime The common starting point for all callback scheduling.
+     * @param period    The period at which to run the callback.
+     * @param offset    The offset from the common starting time.
+     */
+    Callback(std::function<void()> func, std::chrono::microseconds startTime,
+             std::chrono::microseconds period,
+             std::chrono::microseconds offset);
+
+    bool operator>(const Callback& rhs) const {
+      return expirationTime > rhs.expirationTime;
+    }
+  };
+
+  int64_t m_opModeId;
+  bool m_running = true;
+
+  wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
+  std::chrono::microseconds m_startTime;
+  int64_t m_loopStartTimeUs = 0;
+  wpi::units::second_t m_period;
+  Watchdog m_watchdog;
+
+  wpi::util::priority_queue<Callback, std::vector<Callback>,
+                            std::greater<Callback>>
+      m_callbacks;
+
+  void PrintLoopOverrunMessage();
+};
+
+}  // namespace wpi