[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/test/native/cpp/OpModeRobotTest.cpp

@@ -0,0 +1,175 @@
+// 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.
+
+#include "wpi/framework/OpModeRobot.hpp"
+
+#include <sys/types.h>
+
+#include <gtest/gtest.h>
+
+#include "wpi/simulation/DriverStationSim.hpp"
+#include "wpi/simulation/SimHooks.hpp"
+#include "wpi/util/Color.hpp"
+
+namespace {
+class OpModeRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    wpi::sim::PauseTiming();
+    wpi::sim::SetProgramStarted(false);
+  }
+
+  void TearDown() override { wpi::sim::ResumeTiming(); }
+};
+
+class MockRobot;
+
+class MockOpMode : public wpi::OpMode {
+ public:
+  std::atomic<uint32_t> m_disabledPeriodicCount{0};
+  std::atomic<uint32_t> m_opModeRunCount{0};
+  std::atomic<uint32_t> m_opModeStopCount{0};
+
+  MockOpMode() = default;
+  void DisabledPeriodic() override { m_disabledPeriodicCount++; }
+  void OpModeRun(int64_t opModeId) override { m_opModeRunCount++; }
+  void OpModeStop() override { m_opModeStopCount++; }
+};
+
+class OneArgOpMode : public wpi::OpMode {
+ public:
+  explicit OneArgOpMode(MockRobot& robot) {}
+  void OpModeRun(int64_t opModeId) override {}
+  void OpModeStop() override {}
+};
+
+class MockRobot : public wpi::OpModeRobot<MockRobot> {
+ public:
+  std::atomic<uint32_t> m_driverStationConnectedCount{0};
+  std::atomic<uint32_t> m_nonePeriodicCount{0};
+
+  MockRobot() = default;
+
+  void DriverStationConnected() override { m_driverStationConnectedCount++; }
+
+  void NonePeriodic() override { m_nonePeriodicCount++; }
+};
+}  // namespace
+
+static_assert(wpi::ConstructibleOpMode<MockOpMode, MockRobot>);
+static_assert(wpi::ConstructibleOpMode<OneArgOpMode, MockRobot>);
+
+TEST_F(OpModeRobotTest, AddOpMode) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::AUTONOMOUS, "NoArgOpMode-Auto",
+                            "Group", "Description", wpi::util::Color::kWhite,
+                            wpi::util::Color::kBlack);
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TEST, "OneArgOpMode-Test",
+                              "Group", "Description", wpi::util::Color::kWhite,
+                              wpi::util::Color::kBlack);
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  ASSERT_EQ(options.size(), 4u);
+  int indexes[4] = {-1, -1, -1, -1};
+  for (size_t i = 0; i < options.size(); ++i) {
+    auto name = wpi::util::to_string_view(&options[i].name);
+    if (name == "NoArgOpMode-Auto") {
+      indexes[0] = i;
+    } else if (name == "OneArgOpMode-Test") {
+      indexes[1] = i;
+    } else if (name == "NoArgOpMode") {
+      indexes[2] = i;
+    } else if (name == "OneArgOpMode") {
+      indexes[3] = i;
+    }
+  }
+
+  int i = indexes[0];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description");
+  EXPECT_EQ(options[i].textColor, 0xffffff);
+  EXPECT_EQ(options[i].backgroundColor, 0x000000);
+
+  i = indexes[1];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description");
+  EXPECT_EQ(options[i].textColor, 0xffffff);
+  EXPECT_EQ(options[i].backgroundColor, 0x000000);
+
+  i = indexes[2];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "");
+  EXPECT_EQ(options[i].textColor, -1);
+  EXPECT_EQ(options[i].backgroundColor, -1);
+
+  i = indexes[3];
+  ASSERT_NE(i, -1);
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "");
+  EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "");
+  EXPECT_EQ(options[i].textColor, -1);
+  EXPECT_EQ(options[i].backgroundColor, -1);
+}
+
+TEST_F(OpModeRobotTest, ClearOpModes) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  robot.ClearOpModes();
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  EXPECT_TRUE(options.empty());
+}
+
+TEST_F(OpModeRobotTest, RemoveOpMode) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  robot.RemoveOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+  robot.PublishOpModes();
+  auto options = wpi::sim::DriverStationSim::GetOpModeOptions();
+  ASSERT_EQ(options.size(), 1u);
+  EXPECT_EQ(wpi::util::to_string_view(&options[0].name), "OneArgOpMode");
+}
+
+TEST_F(OpModeRobotTest, NonePeriodic) {
+  struct MyMockRobot : public MockRobot {
+    MyMockRobot() {
+      AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
+      AddOpMode<OneArgOpMode>(wpi::RobotMode::TELEOPERATED, "OneArgOpMode");
+      PublishOpModes();
+    }
+  };
+  MyMockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+  wpi::sim::WaitForProgramStart();
+
+  // Time step to get periodic calls on 50 ms timeout
+  wpi::sim::StepTiming(110_ms);
+  EXPECT_EQ(robot.m_nonePeriodicCount.load(), 2u);
+
+  robot.EndCompetition();
+  robotThread.join();
+}

wpilibc/src/test/native/cpp/TimedRobotTest.cpp

@@ -11,6 +11,7 @@
 
 #include <gtest/gtest.h>
 
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/DriverStationSim.hpp"
 #include "wpi/simulation/SimHooks.hpp"
 
@@ -161,8 +162,7 @@ TEST_F(TimedRobotTest, AutonomousMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(true);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -234,8 +234,7 @@ TEST_F(TimedRobotTest, TeleopMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -306,8 +305,7 @@ TEST_F(TimedRobotTest, TestMode) {
   wpi::sim::WaitForProgramStart();
 
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(true);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(1u, robot.m_simulationInitCount);
@@ -369,8 +367,6 @@ TEST_F(TimedRobotTest, TestMode) {
   EXPECT_EQ(0u, robot.m_testExitCount);
 
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
   wpi::sim::StepTiming(20_ms);  // Wait for Notifiers
 
@@ -404,8 +400,6 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Start in disabled
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   EXPECT_EQ(0u, robot.m_disabledInitCount);
@@ -432,8 +426,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to autonomous
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(true);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -450,8 +443,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to teleop
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -468,8 +460,7 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to test
   wpi::sim::DriverStationSim::SetEnabled(true);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(true);
+  wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);
@@ -486,8 +477,6 @@ TEST_F(TimedRobotTest, ModeChange) {
 
   // Transition to disabled
   wpi::sim::DriverStationSim::SetEnabled(false);
-  wpi::sim::DriverStationSim::SetAutonomous(false);
-  wpi::sim::DriverStationSim::SetTest(false);
   wpi::sim::DriverStationSim::NotifyNewData();
 
   wpi::sim::StepTiming(kPeriod);

wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp

@@ -12,7 +12,7 @@
 #include "callback_helpers/TestCallbackHelpers.hpp"
 #include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/driverstation/Joystick.hpp"
-#include "wpi/framework/RobotState.hpp"
+#include "wpi/hal/DriverStationTypes.h"
 #include "wpi/simulation/SimHooks.hpp"
 
 using namespace wpi;
@@ -26,11 +26,11 @@ TEST(DriverStationTest, Enabled) {
   BooleanCallback callback;
   auto cb =
       DriverStationSim::RegisterEnabledCallback(callback.GetCallback(), false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED);
   DriverStationSim::SetEnabled(true);
   DriverStationSim::NotifyNewData();
   EXPECT_TRUE(DriverStationSim::GetEnabled());
   EXPECT_TRUE(DriverStation::IsEnabled());
-  EXPECT_TRUE(RobotState::IsEnabled());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_TRUE(callback.GetLastValue());
 }
@@ -40,16 +40,16 @@ TEST(DriverStationTest, AutonomousMode) {
   DriverStationSim::ResetData();
 
   EXPECT_FALSE(DriverStation::IsAutonomous());
-  BooleanCallback callback;
-  auto cb = DriverStationSim::RegisterAutonomousCallback(callback.GetCallback(),
-                                                         false);
-  DriverStationSim::SetAutonomous(true);
+  EnumCallback callback;
+  auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(),
+                                                        false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS);
   DriverStationSim::NotifyNewData();
-  EXPECT_TRUE(DriverStationSim::GetAutonomous());
+  EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_AUTONOMOUS);
   EXPECT_TRUE(DriverStation::IsAutonomous());
-  EXPECT_TRUE(RobotState::IsAutonomous());
+  EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::AUTONOMOUS);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_TRUE(callback.GetLastValue());
+  EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_AUTONOMOUS);
 }
 
 TEST(DriverStationTest, Mode) {
@@ -57,16 +57,16 @@ TEST(DriverStationTest, Mode) {
   DriverStationSim::ResetData();
 
   EXPECT_FALSE(DriverStation::IsTest());
-  BooleanCallback callback;
-  auto cb =
-      DriverStationSim::RegisterTestCallback(callback.GetCallback(), false);
-  DriverStationSim::SetTest(true);
+  EnumCallback callback;
+  auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(),
+                                                        false);
+  DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST);
   DriverStationSim::NotifyNewData();
-  EXPECT_TRUE(DriverStationSim::GetTest());
+  EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_TEST);
   EXPECT_TRUE(DriverStation::IsTest());
-  EXPECT_TRUE(RobotState::IsTest());
+  EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::TEST);
   EXPECT_TRUE(callback.WasTriggered());
-  EXPECT_TRUE(callback.GetLastValue());
+  EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_TEST);
 }
 
 TEST(DriverStationTest, Estop) {
@@ -81,7 +81,6 @@ TEST(DriverStationTest, Estop) {
   DriverStationSim::NotifyNewData();
   EXPECT_TRUE(DriverStationSim::GetEStop());
   EXPECT_TRUE(DriverStation::IsEStopped());
-  EXPECT_TRUE(RobotState::IsEStopped());
   EXPECT_TRUE(callback.WasTriggered());
   EXPECT_TRUE(callback.GetLastValue());
 }