[examples] Call resetOdometry() when controller command is executed (#5905)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp

@@ -66,10 +66,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
       // Pass the config
       config);
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  frc2::CommandPtr mecanumDriveCommand =
+  frc2::CommandPtr mecanumControllerCommand =
       frc2::MecanumControllerCommand(
           exampleTrajectory, [this]() { return m_drive.GetPose(); },
 
@@ -110,8 +107,16 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
           {&m_drive})
           .ToPtr();
 
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
   return frc2::cmd::Sequence(
-      std::move(mecanumDriveCommand),
-      frc2::InstantCommand([this] { m_drive.Drive(0, 0, 0, false); }, {})
+      frc2::InstantCommand(
+          [this, &exampleTrajectory]() {
+            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          },
+          {})
+          .ToPtr(),
+      std::move(mecanumControllerCommand),
+      frc2::InstantCommand([this]() { m_drive.Drive(0, 0, 0, false); }, {})
           .ToPtr());
 }

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp

@@ -81,10 +81,14 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
       [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive})};
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  return std::move(ramseteCommand)
-      .BeforeStarting(
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
+  return frc2::cmd::RunOnce(
+             [this, &exampleTrajectory] {
+               m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+             },
+             {})
+      .AndThen(std::move(ramseteCommand))
+      .AndThen(
           frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
 }

wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp

@@ -74,10 +74,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   thetaController.EnableContinuousInput(units::radian_t{-std::numbers::pi},
                                         units::radian_t{std::numbers::pi});
 
-  // Reset odometry to the starting pose of the trajectory.
-  m_drive.ResetOdometry(exampleTrajectory.InitialPose());
-
-  frc2::CommandPtr swerveDriveCommand =
+  frc2::CommandPtr swerveControllerCommand =
       frc2::SwerveControllerCommand<4>(
           exampleTrajectory, [this]() { return m_drive.GetPose(); },
 
@@ -92,8 +89,16 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
           {&m_drive})
           .ToPtr();
 
+  // Reset odometry to the initial pose of the trajectory, run path following
+  // command, then stop at the end.
   return frc2::cmd::Sequence(
-      std::move(swerveDriveCommand),
+      frc2::InstantCommand(
+          [this, &exampleTrajectory]() {
+            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          },
+          {})
+          .ToPtr(),
+      std::move(swerveControllerCommand),
       frc2::InstantCommand(
           [this] { m_drive.Drive(0_mps, 0_mps, 0_rad_per_s, false); }, {})
           .ToPtr());