[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/python/pyproject.toml

@@ -94,7 +94,6 @@ MotorControllerGroup = "rpy/MotorControllerGroup.h"
 Notifier = "rpy/Notifier.h"
 
 # wpi/driverstation
-DSControlWord = "wpi/driverstation/DSControlWord.hpp"
 DriverStation = "wpi/driverstation/DriverStation.hpp"
 Gamepad = "wpi/driverstation/Gamepad.hpp"
 GenericHID = "wpi/driverstation/GenericHID.hpp"
@@ -106,8 +105,8 @@ XboxController = "wpi/driverstation/XboxController.hpp"
 
 # wpi/framework
 IterativeRobotBase = "wpi/framework/IterativeRobotBase.hpp"
+OpModeRobot = "wpi/framework/OpModeRobot.hpp"
 RobotBase = "wpi/framework/RobotBase.hpp"
-RobotState = "wpi/framework/RobotState.hpp"
 TimedRobot = "wpi/framework/TimedRobot.hpp"
 TimesliceRobot = "wpi/framework/TimesliceRobot.hpp"
 
@@ -182,6 +181,11 @@ Encoder = "wpi/hardware/rotation/Encoder.hpp"
 # wpi/internal
 DriverStationModeThread = "wpi/internal/DriverStationModeThread.hpp"
 
+# wpi/opmode
+LinearOpMode = "wpi/opmode/LinearOpMode.hpp"
+OpMode = "wpi/opmode/OpMode.hpp"
+PeriodicOpMode = "wpi/opmode/PeriodicOpMode.hpp"
+
 # wpi/smartdashboard
 Field2d = "wpi/smartdashboard/Field2d.hpp"
 FieldObject2d = "wpi/smartdashboard/FieldObject2d.hpp"

wpilibc/src/main/python/semiwrap/DSControlWord.yml

@@ -1,24 +0,0 @@
-classes:
-  wpi::DSControlWord:
-    methods:
-      DSControlWord:
-      IsEnabled:
-        no_release_gil: true
-      IsDisabled:
-        no_release_gil: true
-      IsEStopped:
-        no_release_gil: true
-      IsAutonomous:
-        no_release_gil: true
-      IsAutonomousEnabled:
-        no_release_gil: true
-      IsTeleop:
-        no_release_gil: true
-      IsTeleopEnabled:
-        no_release_gil: true
-      IsTest:
-        no_release_gil: true
-      IsDSAttached:
-        no_release_gil: true
-      IsFMSAttached:
-        no_release_gil: true

wpilibc/src/main/python/semiwrap/DriverStation.yml

@@ -1,6 +1,6 @@
 extra_includes:
-- rpy/ControlWord.h
 - wpi/datalog/DataLog.hpp
+- wpi/util/Color.hpp
 
 classes:
   wpi::DriverStation:
@@ -57,21 +57,23 @@ classes:
       GetStickPOVsAvailable:
       GetStickButtonsMaximumIndex:
       GetStickButtonsAvailable:
+      GetRobotMode:
+      AddOpMode:
+        overloads:
+          RobotMode, std::string_view, std::string_view, std::string_view:
+          RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
+      RemoveOpMode:
+      PublishOpModes:
+      ClearOpModes:
+      GetOpModeId:
+      GetOpMode:
+      IsOpMode:
+        overloads:
+          int64_t:
+          std::string_view:
+      GetControlWord:
       GetStickTouchpadFinger:
       GetStickTouchpadFingerAvailable:
-    inline_code: |
-      .def("getControlState",
-        [](DriverStation *self) -> std::tuple<bool, bool, bool> {
-          py::gil_scoped_release release;
-          return rpy::GetControlState();
-        },
-        py::doc("More efficient way to determine what state the robot is in.\n"
-                "\n"
-                ":returns: booleans representing enabled, isautonomous, istest\n"
-                "\n"
-                ".. versionadded:: 2019.2.1\n"
-                "\n"
-                ".. note:: This function only exists in RobotPy\n"))
   wpi::DriverStation::TouchpadFinger:
     attributes:
       down:

wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml

@@ -3,7 +3,4 @@ classes:
     rename: _DriverStationModeThread
     methods:
       DriverStationModeThread:
-      InAutonomous:
-      InDisabled:
-      InTeleop:
-      InTest:
+      InControl:

wpilibc/src/main/python/semiwrap/LinearOpMode.yml

@@ -0,0 +1,8 @@
+classes:
+  wpi::LinearOpMode:
+    methods:
+      DisabledPeriodic:
+      Run:
+      IsRunning:
+      OpModeRun:
+      OpModeStop:

wpilibc/src/main/python/semiwrap/OpMode.yml

@@ -0,0 +1,6 @@
+classes:
+  wpi::OpMode:
+    methods:
+      DisabledPeriodic:
+      OpModeRun:
+      OpModeStop:

wpilibc/src/main/python/semiwrap/OpModeRobot.yml

@@ -0,0 +1,26 @@
+extra_includes:
+- wpi/util/Color.hpp
+
+classes:
+  wpi::OpModeRobotBase:
+    methods:
+      StartCompetition:
+      EndCompetition:
+      OpModeRobotBase:
+      DriverStationConnected:
+      NonePeriodic:
+      AddOpModeFactory:
+        overloads:
+          OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view:
+          ? OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&
+          :
+      RemoveOpMode:
+      PublishOpModes:
+      ClearOpModes:
+  wpi::OpModeRobot:
+    ignore: true
+    methods:
+      AddOpMode:
+        overloads:
+          RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
+          RobotMode, std::string_view, std::string_view, std::string_view:

wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml

@@ -0,0 +1,17 @@
+classes:
+  wpi::PeriodicOpMode:
+    attributes:
+      kDefaultPeriod:
+    methods:
+      DisabledPeriodic:
+      Start:
+      Periodic:
+      End:
+      GetLoopStartTime:
+      AddPeriodic:
+      GetPeriod:
+      PrintWatchdogEpochs:
+      OpModeRun:
+      OpModeStop:
+      PeriodicOpMode:
+      LoopFunc:

wpilibc/src/main/python/semiwrap/RobotBase.yml

@@ -1,6 +1,5 @@
 extra_includes:
 - wpi/driverstation/DriverStation.hpp
-- rpy/ControlWord.h
 
 functions:
   # TODO
@@ -36,6 +35,8 @@ classes:
       IsReal:
       IsSimulation:
       RobotBase:
+      GetOpModeId:
+      GetOpMode:
     inline_code: |
       .def_static("main",
         [](py::object robot_cls) -> py::object {
@@ -43,20 +44,7 @@ classes:
           auto starter = start.attr("RobotStarter")();
           return starter.attr("run")(robot_cls);
         },
-        py::arg("robot_cls"), py::doc("Starting point for the application"))
-      .def(
-        "getControlState",
-        [](RobotBase *self) -> std::tuple<bool, bool, bool> {
-          py::gil_scoped_release release;
-          return rpy::GetControlState();
-        },
-        py::doc("More efficient way to determine what state the robot is in.\n"
-                "\n"
-                ":returns: booleans representing enabled, isautonomous, istest\n"
-                "\n"
-                ".. versionadded:: 2019.2.1\n"
-                "\n"
-                ".. note:: This function only exists in RobotPy\n"));
+        py::arg("robot_cls"), py::doc("Starting point for the application"));
 
       auto logger = py::module::import("logging").attr("getLogger")("robot");
       cls_RobotBase.attr("logger") = logger;

wpilibc/src/main/python/semiwrap/RobotState.yml

@@ -1,10 +0,0 @@
-classes:
-  wpi::RobotState:
-    nodelete: true
-    methods:
-      IsDisabled:
-      IsEnabled:
-      IsEStopped:
-      IsTeleop:
-      IsAutonomous:
-      IsTest:

wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml

@@ -3,6 +3,8 @@ functions:
     ignore: true
   ConstBufferCallbackStoreThunk:
     ignore: true
+  OpModeOptionsCallbackStoreThunk:
+    ignore: true
 classes:
   wpi::sim::CallbackStore:
     force_type_casters:
@@ -23,4 +25,6 @@ classes:
             ignore: true
           int32_t, int32_t, int32_t, ConstBufferCallback, CancelCallbackChannelFunc:
             ignore: true
+          int32_t, OpModeOptionsCallback, CancelCallbackNoIndexFunc:
+            ignore: true
       SetUid:

wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml

@@ -1,4 +1,6 @@
 classes:
+  wpi::sim::OpModeOptions:
+    ignore: true
   wpi::sim::DriverStationSim:
     force_type_casters:
     - std::function
@@ -6,12 +8,6 @@ classes:
       RegisterEnabledCallback:
       GetEnabled:
       SetEnabled:
-      RegisterAutonomousCallback:
-      GetAutonomous:
-      SetAutonomous:
-      RegisterTestCallback:
-      GetTest:
-      SetTest:
       RegisterEStopCallback:
       GetEStop:
       SetEStop:
@@ -51,3 +47,12 @@ classes:
       SetJoystickPOVsAvailable:
       SetJoystickButtonsMaximumIndex:
       SetJoystickButtonsAvailable:
+      RegisterRobotModeCallback:
+      GetRobotMode:
+      SetRobotMode:
+      RegisterOpModeCallback:
+      GetOpMode:
+      SetOpMode:
+      RegisterOpModeOptionsCallback:
+      GetOpModeOptions:
+        ignore: true

wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml

@@ -3,6 +3,8 @@ functions:
   WaitForProgramStart:
   SetProgramStarted:
   GetProgramStarted:
+  SetProgramState:
+  GetProgramState:
   RestartTiming:
   PauseTiming:
   ResumeTiming:

wpilibc/src/main/python/wpilib/__init__.py

@@ -16,7 +16,6 @@ from ._wpilib import (
     CANStatus,
     Compressor,
     CompressorConfigType,
-    DSControlWord,
     DataLogManager,
     DigitalInput,
     DigitalOutput,
@@ -37,6 +36,7 @@ from ._wpilib import (
     Joystick,
     Koors40,
     LEDPattern,
+    LinearOpMode,
     Mechanism2d,
     MechanismLigament2d,
     MechanismObject2d,
@@ -45,6 +45,9 @@ from ._wpilib import (
     MotorSafety,
     Notifier,
     OnboardIMU,
+    OpMode,
+    OpModeRobotBase,
+    PeriodicOpMode,
     PS4Controller,
     PS5Controller,
     PWM,
@@ -63,7 +66,6 @@ from ._wpilib import (
     Preferences,
     RobotBase,
     RobotController,
-    RobotState,
     RuntimeType,
     SendableBuilderImpl,
     SendableChooser,
@@ -106,7 +108,6 @@ __all__ = [
     "CANStatus",
     "Compressor",
     "CompressorConfigType",
-    "DSControlWord",
     "DataLogManager",
     "DigitalInput",
     "DigitalOutput",
@@ -127,6 +128,7 @@ __all__ = [
     "Joystick",
     "Koors40",
     "LEDPattern",
+    "LinearOpMode",
     "Mechanism2d",
     "MechanismLigament2d",
     "MechanismObject2d",
@@ -135,6 +137,9 @@ __all__ = [
     "MotorSafety",
     "Notifier",
     "OnboardIMU",
+    "OpMode",
+    "OpModeRobotBase",
+    "PeriodicOpMode",
     "PS4Controller",
     "PS5Controller",
     "PWM",
@@ -153,7 +158,6 @@ __all__ = [
     "Preferences",
     "RobotBase",
     "RobotController",
-    "RobotState",
     "RuntimeType",
     "SendableBuilderImpl",
     "SendableChooser",
@@ -191,6 +195,10 @@ __all__ += ["reportError", "reportWarning"]
 
 del _init__wpilib
 
+from .opmoderobot import OpModeRobot
+
+__all__ += ["OpModeRobot"]
+
 from .cameraserver import CameraServer
 from .deployinfo import getDeployData
 

wpilibc/src/main/python/wpilib/opmoderobot.py

@@ -0,0 +1,61 @@
+from hal import RobotMode
+from typing import Optional
+from wpiutil import Color
+
+__all__ = ["OpModeRobot"]
+
+from ._wpilib import OpModeRobotBase, OpMode
+
+class OpModeRobot(OpModeRobotBase):
+    """
+    OpModeRobot implements the opmode-based robot program framework.
+
+    The OpModeRobot class is intended to be subclassed by a user creating a robot
+    program.
+
+    Opmodes are constructed when selected on the driver station, and destroyed
+    when the robot is disabled after being enabled or a different opmode is
+    selected. When no opmode is selected, nonePeriodic() is called. The
+    driverStationConnected() function is called the first time the driver station
+    connects to the robot.
+    """
+
+    def __init__(self):
+        super().__init__()
+
+    def addOpMode(self,
+                  opmodeCls: type,
+                  mode: RobotMode,
+                  name: str,
+                  group: Optional[str] = None,
+                  description: Optional[str] = None,
+                  textColor: Optional[Color] = None,
+                  backgroundColor: Optional[Color] = None) -> None:
+        """
+        Adds an operating mode option. It's necessary to call PublishOpModes() to
+        make the added modes visible to the driver station.
+
+        The textColor and backgroundColor parameters are optional, but setting
+        only one has no effect (if only one is provided, it will be ignored).
+
+        :param opmodeCls: opmode class; must be a public, non-abstract subclass of OpMode
+                          with a constructor that either takes no arguments or accepts a
+                          single argument of this class's type (the latter is preferred).
+        :param mode: robot mode
+        :param name: name of the operating mode
+        :param group: group of the operating mode
+        :param description: description of the operating mode
+        :param textColor: text color
+        :param backgroundColor: background color
+        """
+        def makeOpModeInstance() -> OpMode:
+            # Try to instantiate with robot argument first
+            try:
+                return opmodeCls(self)  # type: ignore
+            except TypeError:
+                # Fallback to no-argument constructor
+                return opmodeCls()  # type: ignore
+        if textColor is None or backgroundColor is None:
+            self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "")
+        else:
+            self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "", textColor, backgroundColor)

wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp

@@ -1,18 +0,0 @@
-#include "rpy/ControlWord.h"
-
-#include "wpi/hal/DriverStation.h"
-
-namespace rpy {
-
-std::tuple<bool, bool, bool> GetControlState() {
-  HAL_ControlWord controlWord;
-  HAL_GetControlWord(&controlWord);
-
-  bool enable = controlWord.enabled != 0 && controlWord.dsAttached != 0;
-  bool auton = controlWord.autonomous != 0;
-  bool test = controlWord.test != 0;
-
-  return std::make_tuple(enable, auton, test);
-}
-
-} // namespace rpy

wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h

@@ -1,7 +0,0 @@
-#include <tuple>
-
-namespace rpy {
-
-std::tuple<bool, bool, bool> GetControlState();
-
-} // namespace rpy