Fix controller command examples, add tests

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

@@ -111,8 +111,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // command, then stop at the end.
   return frc2::cmd::Sequence(
       frc2::InstantCommand(
-          [this, &exampleTrajectory]() {
-            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          [this, initialPose = exampleTrajectory.InitialPose()]() {
+            m_drive.ResetOdometry(initialPose);
           },
           {})
           .ToPtr(),

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

@@ -84,8 +84,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // 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());
+             [this, initialPose = exampleTrajectory.InitialPose()] {
+               m_drive.ResetOdometry(initialPose);
              },
              {})
       .AndThen(std::move(ramseteCommand))

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

@@ -93,8 +93,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
   // command, then stop at the end.
   return frc2::cmd::Sequence(
       frc2::InstantCommand(
-          [this, &exampleTrajectory]() {
-            m_drive.ResetOdometry(exampleTrajectory.InitialPose());
+          [this, initialPose = exampleTrajectory.InitialPose()]() {
+            m_drive.ResetOdometry(initialPose);
           },
           {})
           .ToPtr(),

wpilibcExamples/src/test/cpp/examples/MecanumControllerCommand/cpp/MecanumControllerCommandTest.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class MecanumControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, Match) {
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

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

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <hal/HALBase.h>
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpilibcExamples/src/test/cpp/examples/RamseteCommand/cpp/RamseteCommandTest.cpp

@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class MecanumControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(MecanumControllerCommandTest, Match) {
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

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

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <hal/HALBase.h>
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpilibcExamples/src/test/cpp/examples/SwerveControllerCommand/cpp/SwerveControllerCommandTest.cpp

@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <iostream>
+#include <thread>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/time.h>
+
+#include "Robot.h"
+
+class SwerveControllerCommandTest : public testing::Test {
+  Robot m_robot;
+  std::optional<std::thread> m_thread;
+  bool joystickWarning;
+
+ public:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    joystickWarning = frc::DriverStation::IsJoystickConnectionWarningSilenced();
+    frc::DriverStation::SilenceJoystickConnectionWarning(true);
+
+    m_thread = std::thread([&] { m_robot.StartCompetition(); });
+    frc::sim::StepTiming(0.0_ms);  // Wait for Notifiers
+  }
+
+  void TearDown() override {
+    m_robot.EndCompetition();
+    m_thread->join();
+
+    frc::sim::DriverStationSim::ResetData();
+    frc::DriverStation::SilenceJoystickConnectionWarning(joystickWarning);
+  }
+};
+
+TEST_F(SwerveControllerCommandTest, Match) {
+  std::cerr << "autonomous" << std::endl;
+  // auto
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(15_s);
+
+  // brief disabled period- exact duration shouldn't matter
+  std::cerr << "mid disabled" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(3_s);
+
+  // teleop
+  std::cerr << "teleop" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  frc::sim::StepTiming(135_s);
+
+  // end of match
+  std::cerr << "end of match" << std::endl;
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

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

@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <hal/HALBase.h>
+
+/**
+ * Runs all unit tests.
+ */
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}