From 7913c29114d51b9a87572fb753e677ab3f05de42 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Sun, 10 Mar 2024 20:59:39 -0700 Subject: [PATCH 1/6] Use preview holonomic pose instead of starting rotation --- src/main/java/frc/team2412/robot/util/PathPlannerAutos.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index c9c77f96..35dd74ad 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -69,10 +69,6 @@ private static List getPaths(JsonNode autoJson) { return paths; } - private static Rotation2d getStartingRotation(JsonNode autoJson) { - return Rotation2d.fromDegrees(autoJson.get("startingPose").get("rotation").asDouble()); - } - private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) { return new ChassisSpeeds( state.heading.getCos() * state.velocityMps, @@ -96,7 +92,7 @@ private static List loadAutoTrajectories(String autoName) return List.of(); } List paths = getPaths(autoJson); - Rotation2d startingRotation = getStartingRotation(autoJson); + Rotation2d startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); ChassisSpeeds startingSpeeds = new ChassisSpeeds(); List trajectories = new ArrayList<>(paths.size()); for (var path : paths) { From 1b7721d0897e65d81bd8084b3afc7b6430d0d5f3 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 11 Mar 2024 08:48:34 -0700 Subject: [PATCH 2/6] Shortcut empty auto handling --- src/main/java/frc/team2412/robot/util/PathPlannerAutos.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index 35dd74ad..cb049c3e 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -92,6 +92,9 @@ private static List loadAutoTrajectories(String autoName) return List.of(); } List paths = getPaths(autoJson); + if (paths.isEmpty()) { + return List.of(); + } Rotation2d startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); ChassisSpeeds startingSpeeds = new ChassisSpeeds(); List trajectories = new ArrayList<>(paths.size()); From 8c914d5aa38c86fbad7f5339d669911e15b5e755 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 11 Mar 2024 09:35:37 -0700 Subject: [PATCH 3/6] Add epoch logging of autonomous field --- .../frc/team2412/robot/util/AutonomousField.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 9496cfda..28bb72df 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -5,7 +5,9 @@ import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringSubscriber; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -39,7 +41,19 @@ public static void configureShuffleboardTab( .subscribe(""); var autonomousField = new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); - addPeriodic.accept(() -> autonomousField.update(activeAutoSub.get()), UPDATE_RATE); + var watchdog = + new Watchdog(0.001, () -> DriverStation.reportWarning("auto field loop overrun", false)); + addPeriodic.accept( + () -> { + watchdog.reset(); + autonomousField.update(activeAutoSub.get()); + watchdog.addEpoch("AutonomousField.update()"); + watchdog.disable(); + if (watchdog.isExpired()) { + watchdog.printEpochs(); + } + }, + UPDATE_RATE); tab.add("Selected auto", autonomousField.getField()) .withPosition(columnIndex, rowIndex) .withSize(2, 2); From cdbe6028f2f3ac696ec254f724679c56137e0177 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 11 Mar 2024 14:42:32 -0700 Subject: [PATCH 4/6] Ignore auto field updates when robot is enabled --- src/main/java/frc/team2412/robot/util/AutonomousField.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 28bb72df..ae1076d5 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -129,11 +129,16 @@ public Pose2d getUpdatedPose(String autoName) { } /** - * Updates the {@link Field2d} robot pose. + * Updates the {@link Field2d} robot pose. If the robot is enabled, does nothing and the + * trajectory will restart when the robot is disabled. * * @param autoName The name of the selected PathPlanner autonomous routine. */ public void update(String autoName) { + if (DriverStation.isEnabled()) { + lastName = Optional.empty(); + return; + } field.setRobotPose(getUpdatedPose(autoName)); } } From c9946e0315b34976ea8be28fbbee040a935a6d9d Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Tue, 12 Mar 2024 16:39:21 -0700 Subject: [PATCH 5/6] Add handling for named commands --- .../frc/team2412/robot/util/AutoLogic.java | 2 +- .../frc/team2412/robot/util/AutoPaths.java | 225 ++++++++++-------- .../team2412/robot/util/PathPlannerAutos.java | 50 ++-- 3 files changed, 167 insertions(+), 110 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java index 7c7852d7..8067393b 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/AutoLogic.java @@ -81,7 +81,7 @@ public void registerCommands() { "MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3); - NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4); + NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4); NamedCommands.registerCommand( "TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1); NamedCommands.registerCommand( diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/AutoPaths.java index 039c5fb4..e1dc7da2 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/AutoPaths.java @@ -11,115 +11,152 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; public class AutoPaths { + private static Command registerAuto( + String autoName, Command command, String... primaryPathNames) { + PathPlannerAutos.registerAuto(autoName, primaryPathNames); + return command.withName(autoName); + } // Test Auto - public static SequentialCommandGroup testAuto = - new SequentialCommandGroup( - AutoLogic.getAutoCommand("TestPath"), - new ConditionalCommand( - AutoLogic.getAutoCommand("TestPathTrue"), - AutoLogic.getAutoCommand("TestPathFalse"), - () -> false)); + public static Command testAuto = + registerAuto( + "AutoLogicTest", + new SequentialCommandGroup( + AutoLogic.getAutoCommand("TestPath"), + new ConditionalCommand( + AutoLogic.getAutoCommand("TestPathTrue"), + AutoLogic.getAutoCommand("TestPathFalse"), + () -> false)), + "TestPath", + "TestPathTrue"); // Complex Autos public static Command TopSpeakerCenterLineN1N2AutoLine1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LAutoLineN1")), - Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2AutoLine1", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LAutoLineN1")), + Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command TopSpeakerCenterLineN1N2N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("TopSpeakerQCenterLineN1"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN1LCenterLineN1"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN1QCenterLineN2")), - Commands.sequence( - getAutoCommand("QCenterLineN1QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN2LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), - () -> true)), - () -> true), - VisionLaunchCommand()); + registerAuto( + "TopSpeakerCenterLineN1N2N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("TopSpeakerQCenterLineN1"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN1LCenterLineN1"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN1QCenterLineN2")), + Commands.sequence( + getAutoCommand("QCenterLineN1QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN2LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), + () -> true)), + () -> true), + VisionLaunchCommand()), + "TopSpeakerQCenterLineN1", + "QCenterLineN1LCenterLineN1", + "LCenterLineN1QCenterLineN2"); public static Command midSpeakerCenterLineN3N2N1 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("MidSpeakerQCenterLineN3"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN3LCenterLineN3"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN3QCenterLineN2"), - Commands.either( - Commands.sequence( - getAutoCommand("N3QCenterLineN2LCenterLineN2"), - VisionLaunchCommand(), - getAutoCommand("N3LCenterLineN2LCenterLineN1")), - getAutoCommand("QCenterLineN2LCenterLineN1"), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN3QCenterLineN2"), - getAutoCommand("QCenterLineN2LCenterLineN2")), - () -> true), - VisionLaunchCommand()); + registerAuto( + "MidSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("MidSpeakerQCenterLineN3"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN3LCenterLineN3"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN3QCenterLineN2"), + Commands.either( + Commands.sequence( + getAutoCommand("N3QCenterLineN2LCenterLineN2"), + VisionLaunchCommand(), + getAutoCommand("N3LCenterLineN2LCenterLineN1")), + getAutoCommand("QCenterLineN2LCenterLineN1"), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN3QCenterLineN2"), + getAutoCommand("QCenterLineN2LCenterLineN2")), + () -> true), + VisionLaunchCommand()), + "MidSpeakerQCenterLineN3", + "QCenterLineN3LCenterLineN3", + "LCenterLineN3QCenterLineN2", + "N3QCenterLineN2LCenterLineN2", + "N3LCenterLineN2LCenterLineN1"); public static Command lowSpeakerCenterLineN5N4 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN5"), - Commands.waitSeconds(0.5), - getAutoCommand("LCenterLineN5LCenterLineN4")), - Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), - () -> false)); + registerAuto( + "LowSpeakerCenterLineN5N4", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN5"), + Commands.waitSeconds(0.5), + getAutoCommand("LCenterLineN5LCenterLineN4")), + Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), + () -> false)), + "LowSpeakerQCenterLineN5", + "QCenterLineN5LCenterLineN5", + "LCenterLineN5LCenterLineN4"); public static Command lowSpeakerCenterLineN5N4N3 = - Commands.sequence( - SubwooferLaunchCommand(), - getAutoCommand("LowSpeakerQCenterLineN5"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN5QCenterLineN4"), - Commands.either( - Commands.sequence( - getAutoCommand("QCenterLineN4LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), - () -> true)), - Commands.sequence( - getAutoCommand("QCenterLineN5LCenterLineN4"), - VisionLaunchCommand(), - getAutoCommand("LCenterLineN4LCenterLineN3")), - () -> false), - VisionLaunchCommand()); + registerAuto( + "LowSpeakerCenterLineN5N4N3", + Commands.sequence( + SubwooferLaunchCommand(), + getAutoCommand("LowSpeakerQCenterLineN5"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN5QCenterLineN4"), + Commands.either( + Commands.sequence( + getAutoCommand("QCenterLineN4LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), + () -> true)), + Commands.sequence( + getAutoCommand("QCenterLineN5LCenterLineN4"), + VisionLaunchCommand(), + getAutoCommand("LCenterLineN4LCenterLineN3")), + () -> false), + VisionLaunchCommand()), + "LowSpeakerQCenterLineN5", + "QCenterLineN5QCenterLineN4", + "QCenterLineN4LCenterLineN4", + "LCenterLineN4LCenterLineN3"); // new command getters diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index cb049c3e..9a658f7f 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -17,6 +17,22 @@ import java.util.function.Function; public class PathPlannerAutos { + // Registering named commands + + private static final Map> namedCommandPathsCache = new HashMap<>(); + + public static void registerAuto(String autoName, List paths) { + namedCommandPathsCache.put(autoName, paths); + } + + public static void registerAuto(String autoName, String... pathNames) { + List paths = new ArrayList<>(pathNames.length); + for (String pathName : pathNames) { + paths.add(getPathPlannerPath(pathName)); + } + registerAuto(autoName, paths); + } + // Individual paths private static PathPlannerPath loadPathPlannerPath(String name) { @@ -47,7 +63,8 @@ private static void addPathsFromCommand( case "wait": break; case "named": - // TODO Handle conditional paths (after those are added) + String commandName = commandJson.get("data").get("name").asText(); + paths.addAll(namedCommandPathsCache.getOrDefault(commandName, List.of())); break; case "path": String pathName = commandJson.get("data").get("pathName").asText(); @@ -76,6 +93,22 @@ private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) state.headingAngularVelocityRps); } + private static List trajectoriesFromPaths(List paths) { + if (paths.isEmpty()) { + return List.of(); + } + Rotation2d startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); + ChassisSpeeds startingSpeeds = new ChassisSpeeds(); + List trajectories = new ArrayList<>(paths.size()); + for (var path : paths) { + PathPlannerTrajectory trajectory = path.getTrajectory(startingSpeeds, startingRotation); + trajectories.add(trajectory); + startingRotation = trajectory.getEndState().targetHolonomicRotation; + startingSpeeds = speedsFromState(trajectory.getEndState()); + } + return List.copyOf(trajectories); + } + private static List loadAutoTrajectories(String autoName) { File autoFile = new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); @@ -91,20 +124,7 @@ private static List loadAutoTrajectories(String autoName) DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); return List.of(); } - List paths = getPaths(autoJson); - if (paths.isEmpty()) { - return List.of(); - } - Rotation2d startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); - ChassisSpeeds startingSpeeds = new ChassisSpeeds(); - List trajectories = new ArrayList<>(paths.size()); - for (var path : paths) { - PathPlannerTrajectory trajectory = path.getTrajectory(startingSpeeds, startingRotation); - trajectories.add(trajectory); - startingRotation = trajectory.getEndState().targetHolonomicRotation; - startingSpeeds = speedsFromState(trajectory.getEndState()); - } - return List.copyOf(trajectories); + return trajectoriesFromPaths(getPaths(autoJson)); } private static final Map> autoTrajectoriesCache = From 36f0fa5f8fbe1d0b0c1a4bc5a0fefd8e1ffeb53e Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Tue, 12 Mar 2024 18:26:20 -0700 Subject: [PATCH 6/6] Add starting pose handling --- .../team2412/robot/util/AutonomousField.java | 7 ++- .../team2412/robot/util/PathPlannerAutos.java | 60 ++++++++++++++----- 2 files changed, 51 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index ae1076d5..451dba90 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -63,6 +63,7 @@ public static void configureShuffleboardTab( private final Field2d field = new Field2d(); // Keeping track of the current trajectory + private PathPlannerAutos.Auto auto; private List trajectories; private int trajectoryIndex = 0; @@ -106,12 +107,16 @@ public Pose2d getUpdatedPose(String autoName) { double fpgaTime = Timer.getFPGATimestamp(); if (lastName.isEmpty() || !lastName.get().equals(autoName)) { lastName = Optional.of(autoName); - trajectories = PathPlannerAutos.getAutoTrajectories(autoName); + auto = PathPlannerAutos.getAuto(autoName); + trajectories = auto.trajectories; trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; } if (trajectories.isEmpty()) { + if (auto.startingPose != null) { + return auto.startingPose; + } return new Pose2d(); } lastTrajectoryTimeOffset += (fpgaTime - lastFPGATime) * speed; diff --git a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java index 9a658f7f..f5d924bc 100644 --- a/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java +++ b/src/main/java/frc/team2412/robot/util/PathPlannerAutos.java @@ -4,6 +4,7 @@ import com.fasterxml.jackson.databind.ObjectMapper; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPlannerTrajectory; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; @@ -17,6 +18,21 @@ import java.util.function.Function; public class PathPlannerAutos { + public static final class Auto { + public final Pose2d startingPose; + public final List trajectories; + + public Auto() { + this.startingPose = null; + this.trajectories = List.of(); + } + + public Auto(Pose2d startingPose, List trajectories) { + this.startingPose = startingPose; + this.trajectories = trajectories; + } + } + // Registering named commands private static final Map> namedCommandPathsCache = new HashMap<>(); @@ -93,11 +109,14 @@ private static ChassisSpeeds speedsFromState(PathPlannerTrajectory.State state) state.headingAngularVelocityRps); } - private static List trajectoriesFromPaths(List paths) { + private static List trajectoriesFromPaths( + List paths, Rotation2d startingRotation) { if (paths.isEmpty()) { return List.of(); } - Rotation2d startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); + if (startingRotation == null) { + startingRotation = paths.get(0).getPreviewStartingHolonomicPose().getRotation(); + } ChassisSpeeds startingSpeeds = new ChassisSpeeds(); List trajectories = new ArrayList<>(paths.size()); for (var path : paths) { @@ -109,29 +128,44 @@ private static List trajectoriesFromPaths(List loadAutoTrajectories(String autoName) { + private static Pose2d getStartingPose(JsonNode autoJson) { + JsonNode startingPose = autoJson.get("startingPose"); + if (startingPose.isNull()) { + return null; + } + JsonNode translation = startingPose.get("position"); + return new Pose2d( + translation.get("x").asDouble(), + translation.get("y").asDouble(), + Rotation2d.fromDegrees(startingPose.get("rotation").asDouble())); + } + + private static Auto loadAuto(String autoName) { File autoFile = new File(Filesystem.getDeployDirectory(), "pathplanner/autos/" + autoName + ".auto"); if (!autoFile.exists()) { DriverStation.reportWarning( "Attempted to load non-existent auto \"" + autoName + "\"", false); - return List.of(); + return new Auto(); } JsonNode autoJson; try { autoJson = new ObjectMapper().readTree(autoFile); } catch (IOException e) { DriverStation.reportWarning("Could not load auto \"" + autoName + "\"", e.getStackTrace()); - return List.of(); + return new Auto(); } - return trajectoriesFromPaths(getPaths(autoJson)); + Pose2d startingPose = getStartingPose(autoJson); + Rotation2d startingRotation = startingPose == null ? null : startingPose.getRotation(); + List trajectories = + trajectoriesFromPaths(getPaths(autoJson), startingRotation); + return new Auto(startingPose, trajectories); } - private static final Map> autoTrajectoriesCache = - new HashMap<>(); + private static final Map autosCache = new HashMap<>(); - public static List getAutoTrajectories(String autoName) { - return autoTrajectoriesCache.computeIfAbsent(autoName, PathPlannerAutos::loadAutoTrajectories); + public static Auto getAuto(String autoName) { + return autosCache.computeIfAbsent(autoName, PathPlannerAutos::loadAuto); } // Pre-loading everything @@ -176,10 +210,6 @@ public static void loadAllChoreoTrajectories() { } public static void loadAllAutoTrajectories() { - loadAll( - "pathplanner/autos", - ".auto", - autoTrajectoriesCache, - PathPlannerAutos::loadAutoTrajectories); + loadAll("pathplanner/autos", ".auto", autosCache, PathPlannerAutos::loadAuto); } }