Add pathfindToPoseFlipped auto builder method (#598)

pathplannerlib-python/pathplannerlib/auto.py

@@ -51,7 +51,9 @@ def getCommand(name: str) -> Command:
         if NamedCommands.hasCommand(name):
             return CommandUtil.wrappedEventCommand(NamedCommands._namedCommands[name])
         else:
-            reportWarning(f"PathPlanner attempted to create a command '{name}' that has not been registered with NamedCommands.registerCommand", False)
+            reportWarning(
+                f"PathPlanner attempted to create a command '{name}' that has not been registered with NamedCommands.registerCommand",
+                False)
             return cmd.none()
 
 
@@ -397,19 +399,42 @@ def followPath(path: PathPlannerPath) -> Command:
     def pathfindToPose(pose: Pose2d, constraints: PathConstraints, goal_end_vel: float = 0.0,
                        rotation_delay_distance: float = 0.0) -> Command:
         """
-        Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose rotation and rotation delay distance will have no effect.
+        Build a command to pathfind to a given pose. If not using a holonomic drivetrain, the pose rotation and
+        rotation delay distance will have no effect.
 
         :param pose: The pose to pathfind to
         :param constraints: The constraints to use while pathfinding
         :param goal_end_vel: The goal end velocity of the robot when reaching the target pose
-        :param rotation_delay_distance: The distance the robot should move from the start position before attempting to rotate to the final rotation
+        :param rotation_delay_distance: The distance the robot should move from the start position before attempting
+                to rotate to the final rotation
         :return: A command to pathfind to a given pose
         """
         if not AutoBuilder.isPathfindingConfigured():
             raise RuntimeError('Auto builder was used to build a pathfinding command before being configured')
 
         return AutoBuilder._pathfindToPoseCommandBuilder(pose, constraints, goal_end_vel, rotation_delay_distance)
 
+    @staticmethod
+    def pathfindToPoseFlipped(pose: Pose2d, constraints: PathConstraints, goal_end_vel: float = 0.0,
+                              rotation_delay_distance: float = 0.0) -> Command:
+        """
+        Build a command to pathfind to a given pose that will be flipped based on the value of the path flipping
+        supplier when this command is run. If not using a holonomic drivetrain, the pose rotation and rotation delay
+        distance will have no effect.
+
+        :param pose: The pose to pathfind to. This will be flipped if the path flipping supplier returns true
+        :param constraints: The constraints to use while pathfinding
+        :param goal_end_vel: The goal end velocity of the robot when reaching the target pose
+        :param rotation_delay_distance: The distance the robot should move from the start position before attempting
+                to rotate to the final rotation
+        :return: A command to pathfind to a given pose
+        """
+        return cmd.either(
+            AutoBuilder.pathfindToPose(flipFieldPose(pose), constraints, goal_end_vel, rotation_delay_distance),
+            AutoBuilder.pathfindToPose(pose, constraints, goal_end_vel, rotation_delay_distance),
+            AutoBuilder._shouldFlipPath
+        )
+
     @staticmethod
     def pathfindThenFollowPath(goal_path: PathPlannerPath, pathfinding_constraints: PathConstraints,
                                rotation_delay_distance: float = 0.0) -> Command:

pathplannerlib/src/main/java/com/pathplanner/lib/auto/AutoBuilder.java

@@ -537,6 +537,61 @@ public static Command pathfindToPose(Pose2d pose, PathConstraints constraints) {
     return pathfindToPose(pose, constraints, 0);
   }
 
+  /**
+   * Build a command to pathfind to a given pose that will be flipped based on the value of the path
+   * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose
+   * rotation and rotation delay distance will have no effect.
+   *
+   * @param pose The pose to pathfind to. This will be flipped if the path flipping supplier returns
+   *     true
+   * @param constraints The constraints to use while pathfinding
+   * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose
+   * @param rotationDelayDistance The distance the robot should move from the start position before
+   *     attempting to rotate to the final rotation
+   * @return A command to pathfind to a given pose
+   */
+  public static Command pathfindToPoseFlipped(
+      Pose2d pose,
+      PathConstraints constraints,
+      double goalEndVelocity,
+      double rotationDelayDistance) {
+    return Commands.either(
+        pathfindToPose(
+            GeometryUtil.flipFieldPose(pose), constraints, goalEndVelocity, rotationDelayDistance),
+        pathfindToPose(pose, constraints, goalEndVelocity, rotationDelayDistance),
+        shouldFlipPath);
+  }
+
+  /**
+   * Build a command to pathfind to a given pose that will be flipped based on the value of the path
+   * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose
+   * rotation and rotation delay distance will have no effect.
+   *
+   * @param pose The pose to pathfind to. This will be flipped if the path flipping supplier returns
+   *     true
+   * @param constraints The constraints to use while pathfinding
+   * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose
+   * @return A command to pathfind to a given pose
+   */
+  public static Command pathfindToPoseFlipped(
+      Pose2d pose, PathConstraints constraints, double goalEndVelocity) {
+    return pathfindToPoseFlipped(pose, constraints, goalEndVelocity, 0);
+  }
+
+  /**
+   * Build a command to pathfind to a given pose that will be flipped based on the value of the path
+   * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose
+   * rotation and rotation delay distance will have no effect.
+   *
+   * @param pose The pose to pathfind to. This will be flipped if the path flipping supplier returns
+   *     true
+   * @param constraints The constraints to use while pathfinding
+   * @return A command to pathfind to a given pose
+   */
+  public static Command pathfindToPoseFlipped(Pose2d pose, PathConstraints constraints) {
+    return pathfindToPoseFlipped(pose, constraints, 0);
+  }
+
   /**
    * Build a command to pathfind to a given path, then follow that path. If not using a holonomic
    * drivetrain, the pose rotation delay distance will have no effect.

pathplannerlib/src/main/native/include/pathplanner/lib/auto/AutoBuilder.h

@@ -2,6 +2,7 @@
 
 #include <functional>
 #include <frc2/command/CommandPtr.h>
+#include <frc2/command/Commands.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/ChassisSpeeds.h>
 #include <frc/controller/RamseteController.h>
@@ -206,6 +207,29 @@ class AutoBuilder {
 			PathConstraints constraints, units::meters_per_second_t goalEndVel =
 					0_mps, units::meter_t rotationDelayDistance = 0_m);
 
+	/**
+	 * Build a command to pathfind to a given pose that will be flipped based on the value of the path
+	 * flipping supplier when this command is run. If not using a holonomic drivetrain, the pose
+	 * rotation and rotation delay distance will have no effect.
+	 *
+	 * @param pose The pose to pathfind to. This will be flipped if the path flipping supplier returns
+	 *     true
+	 * @param constraints The constraints to use while pathfinding
+	 * @param goalEndVelocity The goal end velocity of the robot when reaching the target pose
+	 * @param rotationDelayDistance The distance the robot should move from the start position before
+	 *     attempting to rotate to the final rotation
+	 * @return A command to pathfind to a given pose
+	 */
+	static frc2::CommandPtr pathfindToPoseFlipped(frc::Pose2d pose,
+			PathConstraints constraints, units::meters_per_second_t goalEndVel =
+					0_mps, units::meter_t rotationDelayDistance = 0_m) {
+		return frc2::cmd::Either(
+				pathfindToPose(GeometryUtil::flipFieldPose(pose), constraints,
+						goalEndVel, rotationDelayDistance),
+				pathfindToPose(pose, constraints, goalEndVel,
+						rotationDelayDistance), m_shouldFlipPath);
+	}
+
 	/**
 	 * Build a command to pathfind to a given path, then follow that path. If not using a holonomic
 	 * drivetrain, the pose rotation delay distance will have no effect.