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] 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); } }