PathfindThenFollowPath Never Ends

[Captagonal]

Describe the bug
When using PathfindThenFollowPath it sometimes loop overruns when it starts the Joining Path (added in #956 ) and never follows the path. It gets stuck on the command with it never ending leading to me unable to access the drivetrain.

To Reproduce
Steps to reproduce the behavior:

1. Take any example Swerve project (tested on yagsl and CTRE Tuner X generated)
2. Make a small path (see images for mine)
3. Run PathfindThenFollowPath on the path
4. Sometimes when it creates the joining path it has a loop overrun error
   Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:436): Loop time of 0.02s overrun
5. Now it is stuck running the PathfindThenFollowPath with it never ending leaving me unable to cancel the command or schedule a new one on the subsystem.

Expected behavior
It should path find to the path, run the path, and then have the command end so I can schedule new commands on the drivetrain.

Screenshots
Image of my path

Image of the generated joining path

Versions: (please complete the following information):

• OS: [Windows 11]
• GUI Version: [2025.1.1]
• PPLib Version: [2025.1.1]
• PPLib Language: [Java]

[mjansen4857]

This is caused by the path failing to completely generate without something going NaN somewhere. I think I found the cause of that which should hopefully fix this issue.