Strange behavior using pathFindThenFollowPath after switching alliances

[tbeadle]

Describe the bug
We have commands bound to using pathFindThenFollowPath to get the robot lined up at each of the Coral Stations. It seems to work in the simulator until we change alliances. Then we get strange behavior where the robot will make a b-line for the opposite side of the field and then go back. See the attached recording.

Recording 2025-02-11 150723.zip

To Reproduce

public class DriveCommands {
  public enum PrePlannedPath {
    CS_LEFT("Path Find - CS Right", true),
    CS_RIGHT("Path Find - CS Right"),
  ;

    private final String filebase;
    private final boolean isMirrored;

    PrePlannedPath(String filebase, boolean isMirrored) {
      this.filebase = filebase;
      this.isMirrored = isMirrored;
    }

    PrePlannedPath(String filebase) {
      this(filebase, false);
    }

  private static Map<PrePlannedPath, PathPlannerPath> prePlannedPathMap = new HashMap<>();

  static {
    for (PrePlannedPath prePlannedPath : PrePlannedPath.values()) {
      PathPlannerPath path;
      try {
        path = PathPlannerPath.fromPathFile(prePlannedPath.getFilename());
      } catch (Exception e) {
        throw new RuntimeException(e);
      }
      if (prePlannedPath.getIsMirrored()) {
        path = path.mirrorPath();
      }
      prePlannedPathMap.put(prePlannedPath, path);
    }
  }

    ...

  public static Command transformedPathFindToPath(PrePlannedPath path) {
    PathConstraints constraints =
        new PathConstraints(
            MetersPerSecond.of(4.5), MetersPerSecondPerSecond.of(5.0),
            DegreesPerSecond.of(540), DegreesPerSecondPerSecond.of(720));

    final String key = "/Commands/TransformedPathFindToPath/RequestedPath";
    return Commands.runOnce(() -> Logger.recordOutput(key, path.toString()))
        .alongWith(AutoBuilder.pathfindThenFollowPath(prePlannedPathMap.get(path), constraints))
        .finallyDo(() -> Logger.recordOutput(key, ""));
  }

Our AutoBuilder is configured like this:

    AutoBuilder.configure(
        this::getPose,
        this::setPose,
        this::getChassisSpeeds,
        this::runVelocity,
        new PPHolonomicDriveController(
            new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)),
        Constants.robotConfig,
        Utils::allianceIsFlipped,
        this);
    Pathfinding.setPathfinder(new LocalADStarAK());

Utils::allianceIsFlipped is defined like this:

  public static DriverStation.Alliance getAlliance() {
    return DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue);
  }

  public static boolean allianceIsFlipped() {
    return getAlliance() == DriverStation.Alliance.Red;
  }

The path looks like

and here is its json file:

Path Find - CS Right.zip

Now, if we run the simulator, we can use the bindings to move the robot to the appropriate place. Then, if I 'disconnect' in the simulator GUI, change the alliance color (in the video, we change it from red to blue), then go back to teleop and run the commands, you'll see the robot go way across to the other side of the field before coming back to the blue side.

I also have a question, which may be related. Why is it that there seem to be 2 path finding operations when one is a line to a pose near the start of the path and then there's a curvy line to the start of the path, and then the path. And in the case where it's going across the field, that first part does not respect the navgrid obstacles.

Expected behavior
The robot should go directly towards the path that we've specified without going through obstacles.

Versions: (please complete the following information):

• OS: Windows 11
• GUI Version: 2025.2.2
• PPLib Version: 2025.2.2
• PPLib Language: Java

[mjansen4857]

Try switching from the disconnected state to the disabled state before you switch to the enabled state. I’ve seen the simulator do this before because it doesn’t report the right alliance immediately after going from disconnected to enabled. This isn’t an issue on a real robot.

[mjansen4857]

The second path joins the pathfinding path with the target path smoothly, since otherwise it would be a harsh angle most of the time.

[tbeadle]

Unfortunately, that did not seem to help. The robot still does the same weirdness on that first path--going to the other side of the field and not respecting the obstacles.