[wpilibj] Allow passing DS Instance to Robot and OpModes (#8626)

Some discussion with the tech team showed that there were some real
advantages to being able to pass a 2nd type. It allows separating the DS
and Robot. Additionally, we can make the DriverStationBase class
actually usable instead of the existing DriverStation class which is
impossible to handle in intellisense because it has too much.

This won't fully be doable in C++, but we will need to implement
something similar in python.

wpilibj/src/main/java/org/wpilib/driverstation/DefaultUserControls.java

@@ -0,0 +1,31 @@
+// 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.
+
+package org.wpilib.driverstation;
+
+/**
+ * A default implementation of UserControls that provides Gamepad instances for each of the 6
+ * joystick ports provided by the DS.
+ */
+public class DefaultUserControls implements UserControls {
+  private final Gamepad[] m_gamepads;
+
+  /** Constructs a DefaultUserControls instance with Gamepads for each port. */
+  public DefaultUserControls() {
+    m_gamepads = new Gamepad[DriverStation.kJoystickPorts];
+    for (int i = 0; i < m_gamepads.length; i++) {
+      m_gamepads[i] = new Gamepad(i);
+    }
+  }
+
+  /**
+   * Returns the Gamepad instance for the specified port.
+   *
+   * @param port The joystick port number.
+   * @return The Gamepad instance for the given port.
+   */
+  public Gamepad getGamepad(int port) {
+    return m_gamepads[port];
+  }
+}

wpilibj/src/main/java/org/wpilib/driverstation/UserControls.java

@@ -0,0 +1,15 @@
+// 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.
+
+package org.wpilib.driverstation;
+
+/**
+ * An interface representing user controls such as gamepads or joysticks. If your main robot class
+ * has a UserControlsInstance attribute with a class implementing this interface, the constructor is
+ * able to receive an instance of that class. Additionally, any OpModes can also receive that same
+ * instance.
+ *
+ * <p>The implementation of this class must have a default constructor
+ */
+public interface UserControls {}

wpilibj/src/main/java/org/wpilib/driverstation/UserControlsInstance.java

@@ -0,0 +1,26 @@
+// 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.
+
+package org.wpilib.driverstation;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+/**
+ * An annotation to specify the UserControls implementation class to be used for a robot. Apply this
+ * annotation to your main robot class, providing a class that implements the UserControls
+ * interface.
+ */
+@Target(ElementType.TYPE)
+@Retention(RetentionPolicy.RUNTIME)
+public @interface UserControlsInstance {
+  /**
+   * The UserControls implementation class to be used.
+   *
+   * @return The class that implements UserControls.
+   */
+  Class<? extends UserControls> value();
+}

wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java

@@ -6,17 +6,19 @@ package org.wpilib.framework;
 
 import java.io.File;
 import java.io.IOException;
-import java.lang.reflect.Constructor;
 import java.lang.reflect.Modifier;
 import java.net.URL;
 import java.util.Enumeration;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
 import java.util.function.Supplier;
 import java.util.jar.JarEntry;
 import java.util.jar.JarFile;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.UserControls;
+import org.wpilib.driverstation.UserControlsInstance;
 import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
 import org.wpilib.hardware.hal.HAL;
@@ -27,6 +29,7 @@ import org.wpilib.opmode.OpMode;
 import org.wpilib.opmode.Teleop;
 import org.wpilib.opmode.TestOpMode;
 import org.wpilib.util.Color;
+import org.wpilib.util.ConstructorMatch;
 import org.wpilib.util.WPIUtilJNI;
 
 /**
@@ -56,51 +59,68 @@ public abstract class OpModeRobot extends RobotBase {
     DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false);
   }
 
-  /**
-   * Find a public constructor to instantiate the opmode. Prefer a single-arg public constructor
-   * whose parameter type is assignable from this.getClass() (if multiple, pick the most specific
-   * parameter type). Otherwise return the public no-arg constructor. Return null if neither exists.
-   */
-  private Constructor<?> findOpModeConstructor(Class<?> cls) {
-    Constructor<?> bestCtor = null;
-    Class<?> bestParam = null;
-    for (Constructor<?> ctor : cls.getConstructors()) {
-      Class<?>[] params = ctor.getParameterTypes();
-      if (params.length != 1) {
-        continue;
-      }
-      Class<?> param = params[0];
-      if (!param.isAssignableFrom(getClass())) {
-        continue;
-      }
-      if (bestCtor == null || bestParam.isAssignableFrom(param)) {
-        bestCtor = ctor;
-        bestParam = param;
-      }
+  private final Optional<Class<? extends UserControls>> m_userControlsBaseClass;
+  private UserControls m_userControlsInstance;
+
+  void setUserControlsInstance(UserControls userControlsInstance) {
+    if (m_userControlsBaseClass.isEmpty()) {
+      throw new IllegalStateException("No UserControls class specified");
     }
-    if (bestCtor != null) {
-      return bestCtor;
-    }
-    try {
-      return cls.getConstructor();
-    } catch (NoSuchMethodException e) {
-      return null;
+
+    if (!m_userControlsBaseClass.get().isAssignableFrom(userControlsInstance.getClass())) {
+      throw new IllegalArgumentException(
+          userControlsInstance.getClass().getSimpleName()
+              + " is not assignable to "
+              + m_userControlsBaseClass.get().getSimpleName());
     }
+    m_userControlsInstance = userControlsInstance;
   }
 
-  private OpMode constructOpModeClass(Class<?> cls) {
-    Constructor<?> constructor = findOpModeConstructor(cls);
-    if (constructor == null) {
+  /**
+   * Find a public constructor to instantiate the opmode. This constructor can have up to 2
+   * parameters. The first parameter (if present) must be assignable from this.getClass(). The
+   * second parameter (if present) must be assignable from DriverStationBase. If multiple, first
+   * sort by most parameters, then by most specific first, then by most specific second.
+   */
+  private <T> Optional<ConstructorMatch<T>> findOpModeConstructor(Class<T> cls) {
+    Optional<ConstructorMatch<T>> ctor;
+
+    // try 2-parameter constructor
+    ctor = ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with RobotBase parameter
+    ctor = ConstructorMatch.findBestConstructor(cls, getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with UserControls parameter
+    ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass());
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try no-parameter constructor
+    ctor = ConstructorMatch.findBestConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    return Optional.empty();
+  }
+
+  private <T extends OpMode> T constructOpModeClass(Class<T> cls) {
+    Optional<ConstructorMatch<T>> constructor = findOpModeConstructor(cls);
+    if (constructor.isEmpty()) {
       DriverStation.reportError(
           "No suitable constructor to instantiate OpMode " + cls.getSimpleName(), true);
       return null;
     }
     try {
-      if (constructor.getParameterCount() == 1) {
-        return (OpMode) constructor.newInstance(this);
-      } else {
-        return (OpMode) constructor.newInstance();
-      }
+      return constructor.get().newInstance(this, m_userControlsInstance);
     } catch (ReflectiveOperationException e) {
       DriverStation.reportError(
           "Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace());
@@ -128,7 +148,7 @@ public abstract class OpModeRobot extends RobotBase {
     }
     // it must have a public no-arg constructor or a public constructor that accepts this class
     // (or a superclass/interface) as an argument
-    if (findOpModeConstructor(cls) == null) {
+    if (findOpModeConstructor(cls).isEmpty()) {
       throw new IllegalArgumentException(
           "missing public no-arg constructor or constructor accepting "
               + getClass().getSimpleName());
@@ -288,7 +308,7 @@ public abstract class OpModeRobot extends RobotBase {
   }
 
   private void addOpModeClassImpl(
-      Class<?> cls,
+      Class<? extends OpMode> cls,
       RobotMode mode,
       String name,
       String group,
@@ -305,7 +325,7 @@ public abstract class OpModeRobot extends RobotBase {
   }
 
   private void addAnnotatedOpModeImpl(
-      Class<?> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
+      Class<? extends OpMode> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
     checkOpModeClass(cls);
 
     // add an opmode for each annotation
@@ -364,10 +384,10 @@ public abstract class OpModeRobot extends RobotBase {
   private void addAnnotatedOpModeClass(String name) {
     // trim ".class" from end
     String className = name.replace('/', '.').substring(0, name.length() - 6);
-    Class<?> cls;
+    Class<? extends OpMode> cls;
     try {
-      cls = Class.forName(className);
-    } catch (ClassNotFoundException e) {
+      cls = Class.forName(className).asSubclass(OpMode.class);
+    } catch (ClassNotFoundException | ClassCastException e) {
       return;
     }
     Autonomous auto = cls.getAnnotation(Autonomous.class);
@@ -468,6 +488,14 @@ public abstract class OpModeRobot extends RobotBase {
   /** Constructor. */
   @SuppressWarnings("this-escape")
   public OpModeRobot() {
+    // Check to see if we have a DS annotation
+    UserControlsInstance userControlsAnnotation =
+        getClass().getAnnotation(UserControlsInstance.class);
+    if (userControlsAnnotation != null) {
+      m_userControlsBaseClass = Optional.of(userControlsAnnotation.value());
+    } else {
+      m_userControlsBaseClass = Optional.empty();
+    }
     // Scan for annotated opmode classes within the derived class's package and subpackages
     addAnnotatedOpModeClasses(getClass().getPackage());
     DriverStation.publishOpModes();

wpilibj/src/main/java/org/wpilib/framework/RobotBase.java

@@ -4,9 +4,11 @@
 
 package org.wpilib.framework;
 
-import java.lang.reflect.Constructor;
+import java.util.Optional;
 import java.util.concurrent.locks.ReentrantLock;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.UserControls;
+import org.wpilib.driverstation.UserControlsInstance;
 import org.wpilib.hardware.hal.HAL;
 import org.wpilib.hardware.hal.HALUtil;
 import org.wpilib.math.util.MathShared;
@@ -18,6 +20,7 @@ import org.wpilib.system.Notifier;
 import org.wpilib.system.RuntimeType;
 import org.wpilib.system.Timer;
 import org.wpilib.system.WPILibVersion;
+import org.wpilib.util.ConstructorMatch;
 import org.wpilib.util.WPIUtilJNI;
 import org.wpilib.vision.stream.CameraServerShared;
 import org.wpilib.vision.stream.CameraServerSharedStore;
@@ -286,28 +289,29 @@ public abstract class RobotBase implements AutoCloseable {
   private static boolean m_suppressExitWarning;
 
   private static <T extends RobotBase> T constructRobot(Class<T> robotClass) throws Throwable {
-    Constructor<?>[] constructors = robotClass.getConstructors();
-    Constructor<?> defaultConstructor = null;
-    for (Constructor<?> constructor : constructors) {
-      Class<?>[] paramTypes = constructor.getParameterTypes();
-      if (paramTypes.length == 0) {
-        if (defaultConstructor != null) {
-          throw new IllegalArgumentException(
-              "Multiple default constructors found in robot class " + robotClass.getName());
-        }
-        defaultConstructor = constructor;
-      }
+    UserControlsInstance userControlsAttribute =
+        robotClass.getDeclaredAnnotation(UserControlsInstance.class);
+    UserControls userControlsInstance = null;
+    Optional<ConstructorMatch<T>> constructorMatch;
+    if (userControlsAttribute != null) {
+      var userControlsClass = userControlsAttribute.value();
+      userControlsInstance = userControlsClass.getDeclaredConstructor().newInstance();
+      constructorMatch = ConstructorMatch.findBestConstructor(robotClass, userControlsClass);
+    } else {
+      constructorMatch = ConstructorMatch.findBestConstructor(robotClass);
     }
 
-    T robot;
-
-    if (defaultConstructor != null) {
-      robot = robotClass.cast(defaultConstructor.newInstance());
-    } else {
+    if (constructorMatch.isEmpty()) {
       throw new IllegalArgumentException(
           "No valid constructor found in robot class " + robotClass.getName());
     }
 
+    T robot = constructorMatch.get().newInstance(userControlsInstance);
+
+    if (robot instanceof OpModeRobot opModeRobot) {
+      // Insert the UserControls instance into the opModeRobot for use when constructing opmodes
+      opModeRobot.setUserControlsInstance(userControlsInstance);
+    }
     return robot;
   }
 
@@ -392,9 +396,10 @@ public abstract class RobotBase implements AutoCloseable {
   /**
    * Starting point for the applications.
    *
+   * @param <T> Robot subclass.
    * @param robotClass Robot subclass type.
    */
-  public static void startRobot(Class<? extends RobotBase> robotClass) {
+  public static <T extends RobotBase> void startRobot(Class<T> robotClass) {
     // Check that the MSVC runtime is valid.
     WPIUtilJNI.checkMsvcRuntime();