Call to resetOdometry() in getAutonomousCommand() is not made when SwerveControllerCommand is executed as it should

[gartaud]

The call to resetOdometry() in getAutonomousCommand() is not made when SwerveControllerCommand is executed as it should. Rather it is made when getAutonomousCommand() is called.

This is not good as resetOdometry() requires the current pose prior to the execution of the SwerveControllerCommand.

For more info, see discussion at
https://www.chiefdelphi.com/t/how-to-define-the-shortest-trajectory-in-reverse-with-edu-wpi-first-math-trajectory-trajectorygenerator/443549/20

[gartaud]

See also wpilibsuite/allwpilib#5884

[gartaud]

@ElliotScher Can you port your WPILib fix back to this template? Thanks.