Do suggested changes

mjansen4857 · Nov 3, 2023 · 6008cfd · 6008cfd
1 parent 258323b
commit 6008cfd
Show file tree

Hide file tree

Showing 5 changed files with 46 additions and 73 deletions.
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java b/pathplannerlib/src/main/java/com/pathplanner/lib/path/PathPlannerPath.java
@@ -19,27 +19,6 @@
 
 /** A PathPlanner path. NOTE: This is not a trajectory and isn't directly followed. */
 public class PathPlannerPath {
-
-  /** Container for the preview starting state */
-  public static class PreviewStartingState {
-    private Rotation2d rotation;
-    private double velocityMps;
-
-    /** Default constructor */
-    public PreviewStartingState() {}
-
-    /**
-     * Constructor
-     *
-     * @param rotation The rotation of the starting preview poin
-     * @param velocityMps The velocity of the starting preview point
-     */
-    public PreviewStartingState(Rotation2d rotation, double velocityMps) {
-      this.rotation = rotation;
-      this.velocityMps = velocityMps;
-    }
-  }
-
   private List<Translation2d> bezierPoints;
   private List<RotationTarget> rotationTargets;
   private List<ConstraintsZone> constraintZones;
@@ -48,7 +27,7 @@ public PreviewStartingState(Rotation2d rotation, double velocityMps) {
   private GoalEndState goalEndState;
   private List<PathPoint> allPoints;
   private boolean reversed;
-  private PreviewStartingState previewStartingState;
+  private Rotation2d previewStartingRotation;
 
   /**
    * Create a new path planner path
@@ -59,8 +38,8 @@ public PreviewStartingState(Rotation2d rotation, double velocityMps) {
    * @param eventMarkers List of event markers along the path
    * @param globalConstraints The global constraints of the path
    * @param goalEndState The goal end state of the path
-   * @param reversed If the path is reversed. Differential mode only
-   * @param previewStartingState The settings used for previews in the UI
+   * @param reversed Should the robot follow the path reversed (differential drive only)
+   * @param previewStartingRotation The settings used for previews in the UI
    */
   public PathPlannerPath(
       List<Translation2d> bezierPoints,
@@ -70,7 +49,7 @@ public PathPlannerPath(
       PathConstraints globalConstraints,
       GoalEndState goalEndState,
       boolean reversed,
-      PreviewStartingState previewStartingState) {
+      Rotation2d previewStartingRotation) {
     this.bezierPoints = bezierPoints;
     this.rotationTargets = holonomicRotations;
     this.constraintZones = constraintZones;
@@ -79,7 +58,7 @@ public PathPlannerPath(
     this.goalEndState = goalEndState;
     this.reversed = reversed;
     this.allPoints = createPath(this.bezierPoints, this.rotationTargets, this.constraintZones);
-    this.previewStartingState = previewStartingState;
+    this.previewStartingRotation = previewStartingRotation;
 
     precalcValues();
   }
@@ -108,7 +87,7 @@ public PathPlannerPath(
         constraints,
         goalEndState,
         reversed,
-        new PreviewStartingState());
+        Rotation2d.fromDegrees(0));
   }
 
   /**
@@ -135,7 +114,7 @@ private PathPlannerPath(PathConstraints globalConstraints, GoalEndState goalEndS
     this.goalEndState = goalEndState;
     this.reversed = false;
     this.allPoints = new ArrayList<>();
-    this.previewStartingState = new PreviewStartingState();
+    this.previewStartingRotation = Rotation2d.fromDegrees(0);
   }
 
   /**
@@ -231,7 +210,7 @@ public void hotReload(JSONObject pathJson) {
     this.goalEndState = updatedPath.goalEndState;
     this.allPoints = updatedPath.allPoints;
     this.reversed = updatedPath.reversed;
-    this.previewStartingState = updatedPath.previewStartingState;
+    this.previewStartingRotation = updatedPath.previewStartingRotation;
   }
 
   /**
@@ -287,14 +266,12 @@ private static PathPlannerPath fromJson(JSONObject pathJson) {
       eventMarkers.add(EventMarker.fromJson((JSONObject) markerJson));
     }
 
-    PreviewStartingState previewStartingState = new PreviewStartingState();
+    Rotation2d previewStartingRotation = Rotation2d.fromDegrees(0);
     if (pathJson.containsKey("previewStartingState")) {
       JSONObject previewStartingStateJson = (JSONObject) pathJson.get("previewStartingState");
       if (previewStartingStateJson != null) {
-        previewStartingState.rotation =
+        previewStartingRotation =
             Rotation2d.fromDegrees((Double) previewStartingStateJson.get("rotation"));
-        previewStartingState.velocityMps =
-            ((Number) previewStartingStateJson.get("velocity")).doubleValue();
       }
     }
 
@@ -306,7 +283,7 @@ private static PathPlannerPath fromJson(JSONObject pathJson) {
         globalConstraints,
         goalEndState,
         reversed,
-        previewStartingState);
+        previewStartingRotation);
   }
 
   private static List<Translation2d> bezierPointsFromWaypointsJson(JSONArray waypointsJson) {
@@ -358,16 +335,19 @@ public Pose2d getStartingDifferentialPose() {
   }
 
   /**
-   * Get the starting pose for the holomonic path based on the preview settings
+   * Get the starting pose for the holomonic path based on the preview settings.
+   *
+   * <p>NOTE: This should only be used for the first path you are running, and only if you are not
+   * using an auto mode file. Using this pose to reset the robots pose between sequential paths will
+   * cause a loss of accuracy.
    *
    * @return Pose at the path's starting point
    */
-  public Pose2d getStartingHolomonicPreviewPose() {
-    Translation2d startPos = getPoint(0).position;
+  public Pose2d getPreviewStartingHolonomicPose() {
     Rotation2d heading =
-        previewStartingState == null ? Rotation2d.fromDegrees(0) : previewStartingState.rotation;
+        previewStartingRotation == null ? Rotation2d.fromDegrees(0) : previewStartingRotation;
 
-    return new Pose2d(startPos, heading);
+    return new Pose2d(getPoint(0).position, heading);
   }
 
   /**
@@ -620,7 +600,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
           globalConstraints,
           goalEndState,
           reversed,
-          previewStartingState);
+          previewStartingRotation);
     } else if ((closestPointIdx == 0 && robotNextControl == null)
         || (Math.abs(closestDist - startingPose.getTranslation().getDistance(getPoint(0).position))
                 <= 0.25
@@ -682,7 +662,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
             globalConstraints,
             goalEndState,
             reversed,
-            previewStartingState);
+            previewStartingRotation);
       }
     }
 
@@ -716,7 +696,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
           globalConstraints,
           goalEndState,
           reversed,
-          previewStartingState);
+          previewStartingRotation);
     }
 
     if (bezierPoints.isEmpty()) {
@@ -853,7 +833,7 @@ public PathPlannerPath replan(Pose2d startingPose, ChassisSpeeds currentSpeeds)
         globalConstraints,
         goalEndState,
         reversed,
-        previewStartingState);
+        previewStartingRotation);
   }
 
   /**

diff --git a/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp b/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp
@@ -14,11 +14,11 @@ PathPlannerPath::PathPlannerPath(std::vector<frc::Translation2d> bezierPoints,
 		std::vector<ConstraintsZone> constraintZones,
 		std::vector<EventMarker> eventMarkers,
 		PathConstraints globalConstraints, GoalEndState goalEndState,
-		bool reversed, PreviewStartingState previewStartingState) : m_bezierPoints(
+		bool reversed, frc::Rotation2d previewStartingRotation) : m_bezierPoints(
 		bezierPoints), m_rotationTargets(rotationTargets), m_constraintZones(
 		constraintZones), m_eventMarkers(eventMarkers), m_globalConstraints(
-		globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingState(
-		previewStartingState) {
+		globalConstraints), m_goalEndState(goalEndState), m_reversed(reversed), m_previewStartingRotation(
+		previewStartingRotation) {
 	m_allPoints = PathPlannerPath::createPath(m_bezierPoints, m_rotationTargets,
 			m_constraintZones);
 
@@ -42,6 +42,7 @@ void PathPlannerPath::hotReload(const wpi::json &json) {
 	m_goalEndState = updatedPath.m_goalEndState;
 	m_reversed = updatedPath.m_reversed;
 	m_allPoints = updatedPath.m_allPoints;
+	m_previewStartingRotation = updatedPath.m_previewStartingRotation;
 }
 
 std::vector<frc::Translation2d> PathPlannerPath::bezierFromPoses(
@@ -128,16 +129,16 @@ PathPlannerPath PathPlannerPath::fromJson(const wpi::json &json) {
 		eventMarkers.push_back(EventMarker::fromJson(markerJson));
 	}
 
-	PreviewStartingState previewStartingState;
+	frc::Rotation2d previewStartingRotation;
 	if (json.contains("previewStartingState")) {
 		auto jsonStartingState = json["previewStartingState"];
-		previewStartingState = { units::degree_t(jsonStartingState["rotation"]),
-				units::meters_per_second_t(jsonStartingState["velocity"]) };
+		previewStartingRotation = frc::Rotation2d(
+				units::degree_t(jsonStartingState["rotation"]));
 	}
 
 	return PathPlannerPath(bezierPoints, rotationTargets, constraintZones,
 			eventMarkers, globalConstraints, goalEndState, reversed,
-			previewStartingState);
+			previewStartingRotation);
 }
 
 std::vector<frc::Translation2d> PathPlannerPath::bezierPointsFromWaypointsJson(
@@ -247,8 +248,8 @@ frc::Pose2d PathPlannerPath::getStartingDifferentialPose() {
 	return frc::Pose2d(startPos, heading);
 }
 
-frc::Pose2d PathPlannerPath::getStartingHolomonicPreviewPose() {
-	return frc::Pose2d(getPoint(0).position, m_previewStartingState.m_rotation);
+frc::Pose2d PathPlannerPath::getPreviewStartingHolonomicPose() {
+	return frc::Pose2d(getPoint(0).position, m_previewStartingRotation);
 }
 
 void PathPlannerPath::precalcValues() {

diff --git a/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h b/pathplannerlib/src/main/native/include/pathplanner/lib/path/PathPlannerPath.h
@@ -19,16 +19,6 @@
 namespace pathplanner {
 class PathPlannerPath {
 public:
-
-	/**
-	 * Container for the preview starting state
-	 */
-	class PreviewStartingState {
-	public:
-		frc::Rotation2d m_rotation;
-		units::meters_per_second_t m_velocity;
-	};
-
 	/**
 	 * Create a new path planner path
 	 *
@@ -45,8 +35,8 @@ class PathPlannerPath {
 			std::vector<ConstraintsZone> constraintZones,
 			std::vector<EventMarker> eventMarkers,
 			PathConstraints globalConstraints, GoalEndState goalEndState,
-			bool reversed, PreviewStartingState previewStartingState =
-					PreviewStartingState());
+			bool reversed, frc::Rotation2d previewStartingRotation =
+					frc::Rotation2d());
 
 	/**
 	 * Simplified constructor to create a path with no rotation targets, constraint zones, or event
@@ -93,11 +83,13 @@ class PathPlannerPath {
 	frc::Pose2d getStartingDifferentialPose();
 
 	/**
-	 * Get the starting pose for the holomonic path based on the preview settings
+	 * Get the starting pose for the holomonic path based on the preview settings.
+	 *
+	 * NOTE: This should only be used for the first path you are running, and only if you are not using an auto mode file. Using this pose to reset the robots pose between sequential paths will cause a loss of accuracy.
 	 *
 	 * @return Pose at the path's starting point
 	 */
-	frc::Pose2d getStartingHolomonicPreviewPose();
+	frc::Pose2d getPreviewStartingHolonomicPose();
 
 	/**
 	 * Get the constraints for a point along the path
@@ -252,6 +244,6 @@ class PathPlannerPath {
 	GoalEndState m_goalEndState;
 	std::vector<PathPoint> m_allPoints;
 	bool m_reversed;
-	PreviewStartingState m_previewStartingState;
+	frc::Rotation2d m_previewStartingRotation;
 };
 }
diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/path/PathPlannerPathTest.java
@@ -52,8 +52,8 @@ public void testHolomonicStartingPoseSet() {
             new PathConstraints(1, 2, 3, 4),
             new GoalEndState(0, Rotation2d.fromDegrees(0)),
             true,
-            new PathPlannerPath.PreviewStartingState(Rotation2d.fromDegrees(90), 0));
-    Pose2d initialPose = path.getStartingHolomonicPreviewPose();
+            Rotation2d.fromDegrees(90));
+    Pose2d initialPose = path.getPreviewStartingHolonomicPose();
     assertNotNull(initialPose);
     assertEquals(2, initialPose.getX(), EPSILON);
     assertEquals(1, initialPose.getY(), EPSILON);
@@ -76,7 +76,7 @@ public void testHolomonicStartingPoseNotSet() {
             new GoalEndState(0, Rotation2d.fromDegrees(0)),
             true,
             null);
-    Pose2d initialPose = path.getStartingHolomonicPreviewPose();
+    Pose2d initialPose = path.getPreviewStartingHolonomicPose();
     assertNotNull(initialPose);
     assertEquals(2, initialPose.getX(), EPSILON);
     assertEquals(1, initialPose.getY(), EPSILON);

diff --git a/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp b/pathplannerlib/src/test/native/cpp/pathplanner/lib/path/PathPlannerPathTest.cpp
@@ -29,9 +29,9 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseSet)
 			PathConstraints {1_mps, 2_mps_sq, 3_rad_per_s, 4_rad_per_s_sq},
 			GoalEndState(0_mps, 0_deg),
 			true,
-			PathPlannerPath::PreviewStartingState {90_deg, 0_mps});
+			frc::Rotation2d {90_deg});
 
-	frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose();
+	frc::Pose2d initialPose = path.getPreviewStartingHolonomicPose();
 	EXPECT_EQ(2, initialPose.X()());
 	EXPECT_EQ(1, initialPose.Y()());
 	EXPECT_EQ(90, initialPose.Rotation().Degrees()());
@@ -48,7 +48,7 @@ TEST(PathPlannerPathTest, HolomonicStartingPoseNotSet)
 			GoalEndState(0_mps, 0_deg),
 			true);
 
-	frc::Pose2d initialPose = path.getStartingHolomonicPreviewPose();
+	frc::Pose2d initialPose = path.getPreviewStartingHolonomicPose();
 	EXPECT_EQ(2, initialPose.X()());
 	EXPECT_EQ(1, initialPose.Y()());
 	EXPECT_EQ(0, initialPose.Rotation().Degrees()());