Thinking-on-your-feet 🏃 (#257)

* Add thinking-on-your-feet spiky auto

* Add under stage shots

* Tuning

src/main/java/org/littletonrobotics/frc2024/AutoSelector.java

@@ -146,5 +146,6 @@ public static enum AutoQuestionResponse {
     AMP_WALL,
     SCORE_POOPED,
     FOURTH_CENTER,
+    THINKING_ON_YOUR_FEET
   }
 }

src/main/java/org/littletonrobotics/frc2024/RobotContainer.java

@@ -114,8 +114,6 @@ public class RobotContainer {
   private final Trigger autoDriveEnable = overrides.operatorSwitch(3);
   private final Trigger autoFlywheelSpinupDisable = overrides.operatorSwitch(4);
   private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
-  private final Alert ridiculousAutoAlert =
-      new Alert("The selected auto is ridiculous! 😡", AlertType.WARNING);
   private final Alert driverDisconnected =
       new Alert("Driver controller disconnected (port 0).", AlertType.WARNING);
   private final Alert operatorDisconnected =
@@ -364,17 +362,9 @@ private void configureAutos() {
                 "How many spike notes?",
                 List.of(AutoQuestionResponse.TWO, AutoQuestionResponse.THREE)),
             new AutoQuestion(
-                "First center note?",
-                List.of(
-                    AutoQuestionResponse.AMP_WALL,
-                    AutoQuestionResponse.AMP_MIDDLE,
-                    AutoQuestionResponse.MIDDLE)),
+                "First center note?", List.of(AutoQuestionResponse.THINKING_ON_YOUR_FEET)),
             new AutoQuestion(
-                "Second center note?",
-                List.of(
-                    AutoQuestionResponse.AMP_WALL,
-                    AutoQuestionResponse.AMP_MIDDLE,
-                    AutoQuestionResponse.MIDDLE))),
+                "Second center note?", List.of(AutoQuestionResponse.THINKING_ON_YOUR_FEET))),
         autoBuilder.davisSpikyAuto());
     autoSelector.addRoutine("Davis Speedy Auto", List.of(), autoBuilder.davisSpeedyAuto());
     autoSelector.addRoutine("Davis Ethical Auto", autoBuilder.davisEthicalAuto());
@@ -788,11 +778,6 @@ public void updateAlerts() {
       aprilTagLayoutAlert.setText(
           "Non-official AprilTag layout in use (" + getAprilTagLayoutType().toString() + ").");
     }
-
-    // Ridiculous auto alert
-    ridiculousAutoAlert.set(
-        autoSelector.getSelectedName().equals("Davis Spiky Auto")
-            && autoSelector.getResponses().get(2) == autoSelector.getResponses().get(3));
   }
 
   /**

src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import java.util.function.Supplier;
 import org.littletonrobotics.frc2024.FieldConstants;
 import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.subsystems.drive.Drive;
@@ -49,7 +50,13 @@ public static Command resetPose(HolonomicTrajectory trajectory) {
 
   /** Creates a command that follows a trajectory, command ends when the trajectory is finished */
   public static Command followTrajectory(Drive drive, HolonomicTrajectory trajectory) {
-    return startEnd(() -> drive.setTrajectory(trajectory), drive::clearTrajectory)
+    return followTrajectory(drive, () -> trajectory);
+  }
+
+  /** Creates a command that follows a trajectory, command ends when the trajectory is finished */
+  public static Command followTrajectory(
+      Drive drive, Supplier<HolonomicTrajectory> trajectorySupplier) {
+    return startEnd(() -> drive.setTrajectory(trajectorySupplier.get()), drive::clearTrajectory)
         .until(drive::isTrajectoryCompleted);
   }
 

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java

@@ -17,8 +17,10 @@
 import java.nio.file.Path;
 import java.text.DecimalFormat;
 import java.util.HashMap;
+import java.util.HashSet;
 import java.util.List;
 import java.util.Map;
+import java.util.Set;
 import org.littletonrobotics.frc2024.Constants;
 import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
 import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceGrpc;
@@ -49,6 +51,111 @@ public static void main(String[] args) {
                     * 0.75)
             .build();
 
+    // Connect to service
+    var channel =
+        Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build();
+    var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel);
+
+    Set<String> completedPaths = new HashSet<>();
+    while (true) {
+      // Get all paths
+      var allPaths = DriveTrajectories.paths;
+      boolean hasAllPaths = true;
+      for (var supplier : DriveTrajectories.suppliedPaths) {
+        var suppliedPaths = supplier.apply(completedPaths);
+        if (suppliedPaths == null) {
+          hasAllPaths = false;
+        } else {
+          allPaths.putAll(suppliedPaths);
+        }
+      }
+      Set<String> originalKeys = new HashSet<>();
+      originalKeys.addAll(allPaths.keySet());
+      for (String name : originalKeys) {
+        if (completedPaths.contains(name)) {
+          allPaths.remove(name);
+        } else {
+          completedPaths.add(name);
+        }
+      }
+      if (!hasAllPaths && allPaths.size() == 0) {
+        throw new RuntimeException(
+            "Invalid dependency tree. Not all supplied trajectories are available, but there are no trajectories to generate.");
+      }
+
+      // Check hashcodes
+      Map<String, List<PathSegment>> pathQueue = new HashMap<>();
+      for (Map.Entry<String, List<PathSegment>> entry : allPaths.entrySet()) {
+        String hashCode = getHashCode(model, entry.getValue());
+        File pathFile =
+            Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile();
+        if (pathFile.exists()) {
+          try {
+            InputStream fileStream = new FileInputStream(pathFile);
+            Trajectory trajectory = Trajectory.parseFrom(fileStream);
+            if (!trajectory.getHashCode().equals(hashCode)) {
+              pathQueue.put(entry.getKey(), entry.getValue());
+            }
+          } catch (IOException e) {
+            e.printStackTrace();
+          }
+        } else {
+          pathQueue.put(entry.getKey(), entry.getValue());
+        }
+      }
+
+      // Generate trajectories
+      String generateEmptyFlag = System.getenv("GENERATE_EMPTY_DRIVE_TRAJECTORIES");
+      boolean generateEmpty = generateEmptyFlag != null && generateEmptyFlag.length() > 0;
+      for (Map.Entry<String, List<PathSegment>> entry : pathQueue.entrySet()) {
+        Trajectory trajectory;
+        System.out.print(entry.getKey() + " - Generating 💭");
+        double startTime = System.currentTimeMillis();
+        if (generateEmpty) {
+          trajectory =
+              Trajectory.newBuilder()
+                  .addStates(TimestampedVehicleState.newBuilder().build())
+                  .build();
+        } else {
+          // Use service for generation
+          PathRequest request =
+              PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build();
+          TrajectoryResponse response = service.generateTrajectory(request);
+          String error = response.getError().getReason();
+          if (error.length() > 0) {
+            System.err.println(
+                "Got error response for trajectory \"" + entry.getKey() + "\": " + error);
+            System.exit(1);
+          }
+          trajectory =
+              response.getTrajectory().toBuilder()
+                  .setHashCode(getHashCode(model, entry.getValue()))
+                  .build();
+        }
+        File pathFile =
+            Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile();
+        try {
+          OutputStream fileStream = new FileOutputStream(pathFile);
+          trajectory.writeTo(fileStream);
+          double endTime = System.currentTimeMillis();
+          System.out.println(
+              "\r"
+                  + entry.getKey()
+                  + " - Finished in "
+                  + Math.round((endTime - startTime) / 100.0) / 10.0
+                  + " secs ✅");
+        } catch (IOException e) {
+          System.out.println("\r" + entry.getKey() + " - FAILED ⛔");
+          e.printStackTrace();
+        }
+      }
+
+      // Exit if all trajectories ready
+      if (hasAllPaths) {
+        break;
+      }
+    }
+
     // Delete old trajectories
     try {
       Files.list(Path.of("src", "main", "deploy", "trajectories"))
@@ -57,91 +164,15 @@ public static void main(String[] args) {
                 String filename = path.getFileName().toString();
                 if (!filename.endsWith(".pathblob")) return;
                 String[] components = filename.split("\\.");
-                if (components.length == 2
-                    && !DriveTrajectories.paths.keySet().contains(components[0])) {
+                if (components.length == 2 && !completedPaths.contains(components[0])) {
                   path.toFile().delete();
                   System.out.println(components[0] + " - Deleted 💀");
                 }
               });
     } catch (IOException e) {
       e.printStackTrace();
     }
-
-    // Check hashcodes
-    Map<String, List<PathSegment>> pathQueue = new HashMap<>();
-    for (Map.Entry<String, List<PathSegment>> entry : DriveTrajectories.paths.entrySet()) {
-      String hashCode = getHashCode(model, entry.getValue());
-      File pathFile =
-          Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile();
-      if (pathFile.exists()) {
-        try {
-          InputStream fileStream = new FileInputStream(pathFile);
-          Trajectory trajectory = Trajectory.parseFrom(fileStream);
-          if (!trajectory.getHashCode().equals(hashCode)) {
-            pathQueue.put(entry.getKey(), entry.getValue());
-          }
-        } catch (IOException e) {
-          e.printStackTrace();
-        }
-      } else {
-        pathQueue.put(entry.getKey(), entry.getValue());
-      }
-    }
-
-    // Exit if trajectories up-to-date
-    if (pathQueue.isEmpty()) {
-      System.out.println("All trajectories up-to-date!");
-      return;
-    }
-
-    // Connect to service
-    var channel =
-        Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build();
-    var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel);
-    String generateEmptyFlag = System.getenv("GENERATE_EMPTY_DRIVE_TRAJECTORIES");
-    boolean generateEmpty = generateEmptyFlag != null && generateEmptyFlag.length() > 0;
-
-    // Generate trajectories
-    for (Map.Entry<String, List<PathSegment>> entry : pathQueue.entrySet()) {
-      Trajectory trajectory;
-      System.out.print(entry.getKey() + " - Generating 💭");
-      double startTime = System.currentTimeMillis();
-      if (generateEmpty) {
-        trajectory =
-            Trajectory.newBuilder().addStates(TimestampedVehicleState.newBuilder().build()).build();
-      } else {
-        // Use service for generation
-        PathRequest request =
-            PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build();
-        TrajectoryResponse response = service.generateTrajectory(request);
-        String error = response.getError().getReason();
-        if (error.length() > 0) {
-          System.err.println(
-              "Got error response for trajectory \"" + entry.getKey() + "\": " + error);
-          System.exit(1);
-        }
-        trajectory =
-            response.getTrajectory().toBuilder()
-                .setHashCode(getHashCode(model, entry.getValue()))
-                .build();
-      }
-      File pathFile =
-          Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile();
-      try {
-        OutputStream fileStream = new FileOutputStream(pathFile);
-        trajectory.writeTo(fileStream);
-        double endTime = System.currentTimeMillis();
-        System.out.println(
-            "\r"
-                + entry.getKey()
-                + " - Finished in "
-                + Math.round((endTime - startTime) / 100.0) / 10.0
-                + " secs ✅");
-      } catch (IOException e) {
-        System.out.println("\r" + entry.getKey() + " - FAILED ⛔");
-        e.printStackTrace();
-      }
-    }
+    System.out.println("All trajectories up-to-date!");
   }
 
   // create a hashcode for the vehicle model and path segments

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/HolonomicTrajectory.java

@@ -21,22 +21,24 @@
 import java.util.ArrayList;
 import java.util.List;
 import lombok.experimental.ExtensionMethod;
+import org.littletonrobotics.frc2024.Constants;
 import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState;
 
 @ExtensionMethod({TrajectoryGenerationHelpers.class})
 public class HolonomicTrajectory {
   private final Trajectory trajectory;
 
   public HolonomicTrajectory(String name) {
-    File file =
-        Path.of(Filesystem.getDeployDirectory().getPath(), "trajectories", name + ".pathblob")
-            .toFile();
+    Path deployDirectory =
+        Constants.disableHAL
+            ? Path.of("src", "main", "deploy")
+            : Filesystem.getDeployDirectory().toPath();
+    File file = Path.of(deployDirectory.toString(), "trajectories", name + ".pathblob").toFile();
     try {
       InputStream fileStream = new FileInputStream(file);
       trajectory = Trajectory.parseFrom(fileStream);
     } catch (IOException e) {
-      System.err.println("Could not load trajectory \"" + name + "\"");
-      throw new RuntimeException();
+      throw new RuntimeException("Could not load trajectory \"" + name + "\"");
     }
   }
 
@@ -55,14 +57,21 @@ public Pose2d getStartPose() {
 
   public Pose2d[] getTrajectoryPoses() {
     Pose2d[] poses = new Pose2d[trajectory.getStatesCount()];
-
     for (int i = 0; i < trajectory.getStatesCount(); i++) {
       VehicleState state = trajectory.getStates(i).getState();
       poses[i] = new Pose2d(state.getX(), state.getY(), new Rotation2d(state.getTheta()));
     }
     return poses;
   }
 
+  public VehicleState[] getStates() {
+    VehicleState[] states = new VehicleState[trajectory.getStatesCount()];
+    for (int i = 0; i < trajectory.getStatesCount(); i++) {
+      states[i] = trajectory.getStates(i).getState();
+    }
+    return states;
+  }
+
   public VehicleState getStartState() {
     if (trajectory.getStatesCount() == 0) {
       return VehicleState.newBuilder().build();

src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/TrajectoryGenerationHelpers.java

@@ -61,6 +61,22 @@ public static PathSegment.Builder addContinuationWaypoint(
             .setVehicleVelocity(endVelocityConstraint(trajectory)));
   }
 
+  /**
+   * Adds a waypoint to an existing {@link PathSegment.Builder} which is a continuation of a
+   * previously generated vehicle state. The pose and velocity of the waypoint are set to match the
+   * given vehicle state
+   *
+   * @param builder The {@link PathSegment.Builder}.
+   * @param state The generated vehicle state.
+   * @return The {@link PathSegment.Builder} with the new waypoint added.
+   */
+  public static PathSegment.Builder addContinuationWaypoint(
+      PathSegment.Builder builder, VehicleState state) {
+    return builder.addWaypoints(
+        fromPose(Waypoint.newBuilder(), getPose(state))
+            .setVehicleVelocity(createVelocityConstraint(state)));
+  }
+
   /**
    * Adds a translation waypoint to an existing {@link PathSegment.Builder} without setting any
    * other constraints.