Utilize ReplanningConfig in PathfindingCommand (#407)

* java pathfinding replanning config * c++ pathfinding replanning config
mjansen4857 · Oct 16, 2023 · 670e460 · 670e460
1 parent 9169f37
commit 670e460
Show file tree

Hide file tree

Showing 15 changed files with 199 additions and 77 deletions.
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java b/pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java
@@ -143,6 +143,7 @@ public static void configureRamsete(
                 poseSupplier,
                 speedsSupplier,
                 output,
+                replanningConfig,
                 driveSubsystem);
     AutoBuilder.pathfindThenFollowPathCommandBuilder =
         (path, constraints, rotationDelayDistance) ->
@@ -212,6 +213,7 @@ public static void configureRamsete(
                 output,
                 b,
                 zeta,
+                replanningConfig,
                 driveSubsystem);
     AutoBuilder.pathfindThenFollowPathCommandBuilder =
         (path, constraints, rotationDelayDistance) ->
@@ -272,6 +274,7 @@ public static void configureLTV(
                 speedsSupplier,
                 output,
                 dt,
+                replanningConfig,
                 driveSubsystem);
     AutoBuilder.pathfindThenFollowPathCommandBuilder =
         (path, constraints, rotationDelayDistance) ->
@@ -345,6 +348,7 @@ public static void configureLTV(
                 qelems,
                 relems,
                 dt,
+                replanningConfig,
                 driveSubsystem);
     AutoBuilder.pathfindThenFollowPathCommandBuilder =
         (path, constraints, rotationDelayDistance) ->

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindHolonomic.java
@@ -147,6 +147,7 @@ public PathfindHolonomic(
             config.maxModuleSpeed,
             config.driveBaseRadius),
         rotationDelayDistance,
+        config.replanningConfig,
         requirements);
   }
 
@@ -191,6 +192,7 @@ public PathfindHolonomic(
             config.maxModuleSpeed,
             config.driveBaseRadius),
         rotationDelayDistance,
+        config.replanningConfig,
         requirements);
   }
 

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindLTV.java
@@ -3,6 +3,7 @@
 import com.pathplanner.lib.controllers.PPLTVController;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -27,6 +28,7 @@ public class PathfindLTV extends PathfindingCommand {
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -38,6 +40,7 @@ public PathfindLTV(
       Vector<N3> qelems,
       Vector<N2> relems,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         targetPath,
@@ -47,6 +50,7 @@ public PathfindLTV(
         output,
         new PPLTVController(qelems, relems, dt),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -59,6 +63,7 @@ public PathfindLTV(
    * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds
    * @param output a consumer for the output speeds (robot relative)
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -68,6 +73,7 @@ public PathfindLTV(
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         targetPath,
@@ -77,6 +83,7 @@ public PathfindLTV(
         output,
         new PPLTVController(dt),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -93,6 +100,7 @@ public PathfindLTV(
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -105,6 +113,7 @@ public PathfindLTV(
       Vector<N3> qelems,
       Vector<N2> relems,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         new Pose2d(targetPosition, new Rotation2d()),
@@ -115,6 +124,7 @@ public PathfindLTV(
         output,
         new PPLTVController(qelems, relems, dt),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -129,6 +139,7 @@ public PathfindLTV(
    * @param output a consumer for the output speeds (field relative if holonomic, robot relative if
    *     differential)
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -139,6 +150,7 @@ public PathfindLTV(
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         new Pose2d(targetPosition, new Rotation2d()),
@@ -149,6 +161,7 @@ public PathfindLTV(
         output,
         new PPLTVController(dt),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -165,6 +178,7 @@ public PathfindLTV(
    * @param qelems The maximum desired error tolerance for each state.
    * @param relems The maximum desired control effort for each input.
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -176,6 +190,7 @@ public PathfindLTV(
       Vector<N3> qelems,
       Vector<N2> relems,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     this(
         targetPosition,
@@ -187,6 +202,7 @@ public PathfindLTV(
         qelems,
         relems,
         dt,
+        replanningConfig,
         requirements);
   }
 
@@ -201,6 +217,7 @@ public PathfindLTV(
    * @param output a consumer for the output speeds (field relative if holonomic, robot relative if
    *     differential)
    * @param dt Period of the robot control loop in seconds (default 0.02)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindLTV(
@@ -210,6 +227,7 @@ public PathfindLTV(
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
       double dt,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     this(
         targetPosition,
@@ -219,6 +237,7 @@ public PathfindLTV(
         currentRobotRelativeSpeeds,
         output,
         dt,
+        replanningConfig,
         requirements);
   }
 }
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindRamsete.java
@@ -3,6 +3,7 @@
 import com.pathplanner.lib.controllers.PPRamseteController;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -25,6 +26,7 @@ public class PathfindRamsete extends PathfindingCommand {
    *     aggressive like a proportional term.
    * @param zeta Tuning parameter (0 rad^-1 &lt; zeta &lt; 1 rad^-1) for which larger values provide
    *     more damping in response.
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -35,6 +37,7 @@ public PathfindRamsete(
       Consumer<ChassisSpeeds> output,
       double b,
       double zeta,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         targetPath,
@@ -44,6 +47,7 @@ public PathfindRamsete(
         output,
         new PPRamseteController(b, zeta),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -55,6 +59,7 @@ public PathfindRamsete(
    * @param poseSupplier a supplier for the robot's current pose
    * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds
    * @param output a consumer for the output speeds (robot relative)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -63,6 +68,7 @@ public PathfindRamsete(
       Supplier<Pose2d> poseSupplier,
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         targetPath,
@@ -72,6 +78,7 @@ public PathfindRamsete(
         output,
         new PPRamseteController(),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -89,6 +96,7 @@ public PathfindRamsete(
    *     aggressive like a proportional term.
    * @param zeta Tuning parameter (0 rad^-1 &lt; zeta &lt; 1 rad^-1) for which larger values provide
    *     more damping in response.
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -100,6 +108,7 @@ public PathfindRamsete(
       Consumer<ChassisSpeeds> output,
       double b,
       double zeta,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         new Pose2d(targetPosition, new Rotation2d()),
@@ -110,6 +119,7 @@ public PathfindRamsete(
         output,
         new PPRamseteController(b, zeta),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -123,6 +133,7 @@ public PathfindRamsete(
    * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds
    * @param output a consumer for the output speeds (field relative if holonomic, robot relative if
    *     differential)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -132,6 +143,7 @@ public PathfindRamsete(
       Supplier<Pose2d> poseSupplier,
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     super(
         new Pose2d(targetPosition, new Rotation2d()),
@@ -142,6 +154,7 @@ public PathfindRamsete(
         output,
         new PPRamseteController(),
         0,
+        replanningConfig,
         requirements);
   }
 
@@ -159,6 +172,7 @@ public PathfindRamsete(
    *     aggressive like a proportional term.
    * @param zeta Tuning parameter (0 rad^-1 &lt; zeta &lt; 1 rad^-1) for which larger values provide
    *     more damping in response.
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -169,6 +183,7 @@ public PathfindRamsete(
       Consumer<ChassisSpeeds> output,
       double b,
       double zeta,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     this(
         targetPosition,
@@ -179,6 +194,7 @@ public PathfindRamsete(
         output,
         b,
         zeta,
+        replanningConfig,
         requirements);
   }
 
@@ -192,6 +208,7 @@ public PathfindRamsete(
    * @param currentRobotRelativeSpeeds a supplier for the robot's current robot relative speeds
    * @param output a consumer for the output speeds (field relative if holonomic, robot relative if
    *     differential)
+   * @param replanningConfig Path replanning configuration
    * @param requirements the subsystems required by this command
    */
   public PathfindRamsete(
@@ -200,6 +217,7 @@ public PathfindRamsete(
       Supplier<Pose2d> poseSupplier,
       Supplier<ChassisSpeeds> currentRobotRelativeSpeeds,
       Consumer<ChassisSpeeds> output,
+      ReplanningConfig replanningConfig,
       Subsystem... requirements) {
     this(
         targetPosition,
@@ -208,6 +226,7 @@ public PathfindRamsete(
         poseSupplier,
         currentRobotRelativeSpeeds,
         output,
+        replanningConfig,
         requirements);
   }
 }
diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPathLTV.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPathLTV.java
@@ -50,6 +50,7 @@ public PathfindThenFollowPathLTV(
             qelems,
             relems,
             dt,
+            replanningConfig,
             requirements),
         new FollowPathWithEvents(
             new FollowPathLTV(
@@ -95,6 +96,7 @@ public PathfindThenFollowPathLTV(
             currentRobotRelativeSpeeds,
             robotRelativeOutput,
             dt,
+            replanningConfig,
             requirements),
         new FollowPathWithEvents(
             new FollowPathLTV(

diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPathRamsete.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PathfindThenFollowPathRamsete.java
@@ -46,6 +46,7 @@ public PathfindThenFollowPathRamsete(
             robotRelativeOutput,
             b,
             zeta,
+            replanningConfig,
             requirements),
         new FollowPathWithEvents(
             new FollowPathRamsete(
@@ -87,6 +88,7 @@ public PathfindThenFollowPathRamsete(
             poseSupplier,
             currentRobotRelativeSpeeds,
             robotRelativeOutput,
+            replanningConfig,
             requirements),
         new FollowPathRamsete(
             goalPath,