[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>

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

@@ -18,11 +18,13 @@
 
 #include "HALInitializer.h"
 #include "mockdata/DriverStationDataInternal.h"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/Errors.h"
 #include "wpi/hal/cpp/fpga_clock.h"
 #include "wpi/hal/simulation/MockHooks.h"
 #include "wpi/util/EventVector.hpp"
 #include "wpi/util/mutex.hpp"
+#include "wpi/util/string.h"
 
 static wpi::util::mutex msgMutex;
 static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
@@ -69,15 +71,10 @@ void JoystickDataCache::Update() {
   allianceStation = SimDriverStationData->allianceStationId;
   matchTime = SimDriverStationData->matchTime;
 
-  HAL_ControlWord tmpControlWord;
-  std::memset(&tmpControlWord, 0, sizeof(tmpControlWord));
-  tmpControlWord.enabled = SimDriverStationData->enabled;
-  tmpControlWord.autonomous = SimDriverStationData->autonomous;
-  tmpControlWord.test = SimDriverStationData->test;
-  tmpControlWord.eStop = SimDriverStationData->eStop;
-  tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached;
-  tmpControlWord.dsAttached = SimDriverStationData->dsAttached;
-  this->controlWord = tmpControlWord;
+  controlWord = HAL_MakeControlWord(
+      SimDriverStationData->opMode, SimDriverStationData->robotMode,
+      SimDriverStationData->enabled, SimDriverStationData->eStop,
+      SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached);
 }
 
 #define CHECK_JOYSTICK_NUMBER(stickNum)                  \
@@ -221,6 +218,7 @@ int32_t HAL_SendConsoleLine(const char* line) {
 
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   if (gShutdown) {
+    controlWord->value = 0;
     return INCOMPATIBLE_STATE;
   }
   std::scoped_lock lock{driverStation->cacheMutex};
@@ -228,6 +226,35 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
   return 0;
 }
 
+int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
+  if (gShutdown) {
+    controlWord->value = 0;
+    return INCOMPATIBLE_STATE;
+  }
+  bool dsAttached = SimDriverStationData->dsAttached;
+  if (dsAttached) {
+    *controlWord = HAL_MakeControlWord(
+        SimDriverStationData->opMode, SimDriverStationData->robotMode,
+        SimDriverStationData->enabled, SimDriverStationData->eStop,
+        SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached);
+  } else {
+    controlWord->value = 0;
+  }
+  return 0;
+}
+
+int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
+                             int32_t count) {
+  if (gShutdown) {
+    return 0;
+  }
+  if (count < 0 || count > 1000 || (count != 0 && !options)) {
+    return PARAMETER_OUT_OF_RANGE;
+  }
+  SimDriverStationData->SetOpModeOptions({options, options + count});
+  return 0;
+}
+
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
   if (gShutdown) {
     return HAL_AllianceStationID_kUnknown;
@@ -370,20 +397,8 @@ void HAL_ObserveUserProgramStarting(void) {
   HALSIM_SetProgramStarted(true);
 }
 
-void HAL_ObserveUserProgramDisabled(void) {
-  // TODO
-}
-
-void HAL_ObserveUserProgramAutonomous(void) {
-  // TODO
-}
-
-void HAL_ObserveUserProgramTeleop(void) {
-  // TODO
-}
-
-void HAL_ObserveUserProgramTest(void) {
-  // TODO
+void HAL_ObserveUserProgram(HAL_ControlWord word) {
+  HALSIM_SetProgramState(word);
 }
 
 HAL_Bool HAL_RefreshDSData(void) {
@@ -415,8 +430,7 @@ HAL_Bool HAL_RefreshDSData(void) {
       // Also, when the DS has never been connected the rest of the fields
       // in control word are garbage, so we also need to zero out in that
       // case too
-      std::memset(&currentRead->controlWord, 0,
-                  sizeof(currentRead->controlWord));
+      currentRead->controlWord.value = 0;
     }
     newestControlWord = currentRead->controlWord;
   }
@@ -450,7 +464,8 @@ HAL_Bool HAL_GetOutputsEnabled(void) {
     return false;
   }
   std::scoped_lock lock{driverStation->cacheMutex};
-  return newestControlWord.enabled && newestControlWord.dsAttached;
+  return HAL_ControlWord_IsEnabled(newestControlWord) &&
+         HAL_ControlWord_IsDSAttached(newestControlWord);
 }
 
 }  // extern "C"

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

@@ -15,6 +15,7 @@
 #include "wpi/util/timestamp.h"
 
 static std::atomic<bool> programStarted{false};
+static std::atomic<int64_t> programState{0};
 
 static std::atomic<uint64_t> programStartTime{0};
 static std::atomic<uint64_t> programPauseTime{0};
@@ -98,6 +99,14 @@ HAL_Bool HALSIM_GetProgramStarted(void) {
   return GetProgramStarted();
 }
 
+void HALSIM_SetProgramState(HAL_ControlWord controlWord) {
+  programState = controlWord.value;
+}
+
+void HALSIM_GetProgramState(HAL_ControlWord* controlWord) {
+  controlWord->value = programState;
+}
+
 void HALSIM_RestartTiming(void) {
   RestartTiming();
 }

hal/src/main/native/sim/mockdata/DriverStationData.cpp

@@ -2,14 +2,30 @@
 // 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.
 
+#include "wpi/hal/simulation/DriverStationData.h"
+
+#include <algorithm>
 #include <cstring>
+#include <string>
+#include <vector>
+
+#include <fmt/format.h>
 
 #include "DriverStationDataInternal.h"
+#include "wpi/hal/DashboardOpMode.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 
 using namespace wpi::hal;
 
+static void FreeOpModeOption(HAL_OpModeOption& option) {
+  WPI_FreeString(&option.name);
+  WPI_FreeString(&option.group);
+  WPI_FreeString(&option.description);
+}
+
 namespace wpi::hal::init {
 void InitializeDriverStationData() {
+  wpi::hal::InitializeDashboardOpMode();
   static DriverStationData dsd;
   ::wpi::hal::SimDriverStationData = &dsd;
 }
@@ -21,15 +37,21 @@ DriverStationData::DriverStationData() {
   ResetData();
 }
 
+DriverStationData::~DriverStationData() {
+  for (auto&& option : m_opModeOptions) {
+    FreeOpModeOption(option);
+  }
+}
+
 void DriverStationData::ResetData() {
   enabled.Reset(false);
-  autonomous.Reset(false);
-  test.Reset(false);
+  robotMode.Reset(HAL_ROBOTMODE_UNKNOWN);
   eStop.Reset(false);
   fmsAttached.Reset(false);
   dsAttached.Reset(false);
   allianceStationId.Reset(static_cast<HAL_AllianceStationID>(0));
   matchTime.Reset(-1.0);
+  opMode.Reset(0);
 
   {
     std::scoped_lock lock(m_joystickDataMutex);
@@ -61,7 +83,72 @@ void DriverStationData::ResetData() {
     m_matchInfoCallbacks.Reset();
     m_matchInfo = HAL_MatchInfo{};
   }
+  {
+    std::scoped_lock lock{m_opModeMutex};
+    m_opModeOptionsCallbacks.Reset();
+    // XXX: do not clear options vector as it comes from robot code?
+  }
   m_newDataCallbacks.Reset();
+  wpi::hal::SetDashboardOpModeOptions({});
+}
+
+void DriverStationData::SetOpModeOptions(
+    std::span<const HAL_OpModeOption> options) {
+  std::scoped_lock lock{m_opModeMutex};
+
+  for (auto&& option : m_opModeOptions) {
+    FreeOpModeOption(option);
+  }
+  m_opModeOptions.clear();
+  m_opModeOptions.reserve(options.size());
+  for (const auto& option : options) {
+    if (option.id == 0) {
+      continue;
+    }
+    m_opModeOptions.emplace_back(
+        HAL_OpModeOption{static_cast<int64_t>(option.id),
+                         wpi::util::copy_wpi_string(option.name),
+                         wpi::util::copy_wpi_string(option.group),
+                         wpi::util::copy_wpi_string(option.description),
+                         option.textColor, option.backgroundColor});
+  }
+  m_opModeOptionsCallbacks.Invoke(m_opModeOptions.data(),
+                                  m_opModeOptions.size());
+  wpi::hal::SetDashboardOpModeOptions(options);
+}
+
+int32_t DriverStationData::RegisterOpModeOptionsCallback(
+    HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify) {
+  std::scoped_lock lock(m_opModeMutex);
+  int32_t uid = m_opModeOptionsCallbacks.Register(callback, param);
+  if (initialNotify) {
+    callback(GetOpModeOptionsName(), param, m_opModeOptions.data(),
+             m_opModeOptions.size());
+  }
+  return uid;
+}
+
+void DriverStationData::CancelOpModeOptionsCallback(int32_t uid) {
+  m_opModeOptionsCallbacks.Cancel(uid);
+}
+
+HAL_OpModeOption* DriverStationData::GetOpModeOptions(int32_t* len) {
+  std::scoped_lock lock(m_opModeMutex);
+  *len = 0;
+  if (m_opModeOptions.empty()) {
+    return nullptr;
+  }
+
+  auto options = static_cast<HAL_OpModeOption*>(
+      std::malloc(sizeof(HAL_OpModeOption) * m_opModeOptions.size()));
+  std::copy(m_opModeOptions.begin(), m_opModeOptions.end(), options);
+  *len = m_opModeOptions.size();
+  for (auto&& option : std::span{options, m_opModeOptions.size()}) {
+    option.name = wpi::util::copy_wpi_string(option.name);
+    option.group = wpi::util::copy_wpi_string(option.group);
+    option.description = wpi::util::copy_wpi_string(option.description);
+  }
+  return options;
 }
 
 #define DEFINE_CPPAPI_CALLBACKS(name, data, data2)                             \
@@ -495,13 +582,38 @@ void HALSIM_ResetDriverStationData(void) {
                                        SimDriverStationData, LOWERNAME)
 
 DEFINE_CAPI(HAL_Bool, Enabled, enabled)
-DEFINE_CAPI(HAL_Bool, Autonomous, autonomous)
-DEFINE_CAPI(HAL_Bool, Test, test)
+DEFINE_CAPI(HAL_RobotMode, RobotMode, robotMode)
 DEFINE_CAPI(HAL_Bool, EStop, eStop)
 DEFINE_CAPI(HAL_Bool, FmsAttached, fmsAttached)
 DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached)
 DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId)
 DEFINE_CAPI(double, MatchTime, matchTime)
+DEFINE_CAPI(int64_t, OpMode, opMode)
+
+int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
+                                             void* param,
+                                             HAL_Bool initialNotify) {
+  return SimDriverStationData->RegisterOpModeOptionsCallback(callback, param,
+                                                             initialNotify);
+}
+
+void HALSIM_CancelOpModeOptionsCallback(int32_t uid) {
+  return SimDriverStationData->CancelOpModeOptionsCallback(uid);
+}
+
+struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len) {
+  return SimDriverStationData->GetOpModeOptions(len);
+}
+
+void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr,
+                                   size_t length) {
+  for (size_t i = 0; i < length; ++i) {
+    WPI_FreeString(&arr[i].name);
+    WPI_FreeString(&arr[i].group);
+    WPI_FreeString(&arr[i].description);
+  }
+  std::free(arr);
+}
 
 #undef DEFINE_CAPI
 #define DEFINE_CAPI(name, data)                                                \
@@ -704,8 +816,7 @@ void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback,
                                               void* param,
                                               HAL_Bool initialNotify) {
   REGISTER(enabled);
-  REGISTER(autonomous);
-  REGISTER(test);
+  REGISTER(robotMode);
   REGISTER(eStop);
   REGISTER(fmsAttached);
   REGISTER(dsAttached);

hal/src/main/native/sim/mockdata/DriverStationDataInternal.h

@@ -4,8 +4,13 @@
 
 #pragma once
 
-#include <memory>
+#include <stdint.h>
 
+#include <span>
+#include <string_view>
+#include <vector>
+
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/hal/simulation/DriverStationData.h"
 #include "wpi/hal/simulation/SimCallbackRegistry.h"
 #include "wpi/hal/simulation/SimDataValue.h"
@@ -15,13 +20,14 @@ namespace wpi::hal {
 
 class DriverStationData {
   HAL_SIMDATAVALUE_DEFINE_NAME(Enabled)
-  HAL_SIMDATAVALUE_DEFINE_NAME(Autonomous)
-  HAL_SIMDATAVALUE_DEFINE_NAME(Test)
+  HAL_SIMDATAVALUE_DEFINE_NAME(RobotMode)
   HAL_SIMDATAVALUE_DEFINE_NAME(EStop)
   HAL_SIMDATAVALUE_DEFINE_NAME(FmsAttached)
   HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached)
   HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId)
   HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime)
+  HAL_SIMDATAVALUE_DEFINE_NAME(OpMode)
+  HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(OpModeOptions)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickAxes)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickPOVs)
   HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickButtons)
@@ -36,11 +42,25 @@ class DriverStationData {
   MakeAllianceStationIdValue(HAL_AllianceStationID value) {
     return HAL_MakeEnum(value);
   }
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value
+  MakeRobotModeValue(HAL_RobotMode value) {
+    return HAL_MakeEnum(value);
+  }
 
  public:
   DriverStationData();
+  ~DriverStationData();
+  DriverStationData(const DriverStationData&) = delete;
+  DriverStationData& operator=(const DriverStationData&) = delete;
   void ResetData();
 
+  void SetOpModeOptions(std::span<const HAL_OpModeOption> options);
+
+  int32_t RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback,
+                                        void* param, HAL_Bool initialNotify);
+  void CancelOpModeOptionsCallback(int32_t uid);
+  HAL_OpModeOption* GetOpModeOptions(int32_t* len);
+
   int32_t RegisterJoystickAxesCallback(int32_t joystickNum,
                                        HAL_JoystickAxesCallback callback,
                                        void* param, HAL_Bool initialNotify);
@@ -141,8 +161,8 @@ class DriverStationData {
   void SetReplayNumber(int32_t replayNumber);
 
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEnabledName> enabled{false};
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetAutonomousName> autonomous{false};
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetTestName> test{false};
+  SimDataValue<HAL_RobotMode, MakeRobotModeValue, GetRobotModeName> robotMode{
+      HAL_ROBOTMODE_UNKNOWN};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetEStopName> eStop{false};
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFmsAttachedName> fmsAttached{
       false};
@@ -151,8 +171,11 @@ class DriverStationData {
                GetAllianceStationIdName>
       allianceStationId{static_cast<HAL_AllianceStationID>(0)};
   SimDataValue<double, HAL_MakeDouble, GetMatchTimeName> matchTime{-1.0};
+  SimDataValue<int64_t, HAL_MakeLong, GetOpModeName> opMode{0};
 
  private:
+  SimCallbackRegistry<HAL_OpModeOptionsCallback, GetOpModeOptionsName>
+      m_opModeOptionsCallbacks;
   SimCallbackRegistry<HAL_JoystickAxesCallback, GetJoystickAxesName>
       m_joystickAxesCallbacks;
   SimCallbackRegistry<HAL_JoystickPOVsCallback, GetJoystickPOVsName>
@@ -194,6 +217,9 @@ class DriverStationData {
 
   wpi::util::spinlock m_matchInfoMutex;
   HAL_MatchInfo m_matchInfo;
+
+  wpi::util::spinlock m_opModeMutex;
+  std::vector<HAL_OpModeOption> m_opModeOptions;
 };
 extern DriverStationData* SimDriverStationData;
 }  // namespace wpi::hal