fix sysid crashing when passed invalid function

wpilibsuite · Apr 10, 2024 · c05dbef · c05dbef
1 parent 33f12f0
commit c05dbef
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 6 deletions.
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
@@ -52,7 +52,8 @@ class Config {
   Config(std::optional<ramp_rate_t> rampRate,
          std::optional<units::volt_t> stepVoltage,
          std::optional<units::second_t> timeout,
-         std::optional<std::function<void(frc::sysid::State)>> recordState) {
+         std::function<void(frc::sysid::State)> recordState)
+      : m_recordState{recordState} {
     if (rampRate) {
       m_rampRate = rampRate.value();
     }
@@ -62,9 +63,6 @@ class Config {
     if (timeout) {
       m_timeout = timeout.value();
     }
-    if (recordState) {
-      m_recordState = recordState.value();
-    }
   }
 };
 
@@ -109,7 +107,7 @@ class Mechanism {
             std::function<void(frc::sysid::SysIdRoutineLog*)> log,
             frc2::Subsystem* subsystem, std::string_view name)
       : m_drive{std::move(drive)},
-        m_log{std::move(log)},
+        m_log{log ? std::move(log) : [](frc::sysid::SysIdRoutineLog* log) {}},
         m_subsystem{subsystem},
         m_name{name} {}
 
@@ -134,7 +132,7 @@ class Mechanism {
             std::function<void(frc::sysid::SysIdRoutineLog*)> log,
             frc2::Subsystem* subsystem)
       : m_drive{std::move(drive)},
-        m_log{std::move(log)},
+        m_log{log ? std::move(log) : [](frc::sysid::SysIdRoutineLog* log) {}},
         m_subsystem{subsystem},
         m_name{m_subsystem->GetName()} {}
 };

diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
@@ -63,6 +63,7 @@ class SysIdRoutineTest : public ::testing::Test {
             log->Motor("Mock Motor").position(0_m).velocity(0_mps).voltage(0_V);
           },
           &m_subsystem}};
+
   frc2::CommandPtr m_quasistaticForward{
       m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
   frc2::CommandPtr m_quasistaticReverse{
@@ -72,6 +73,14 @@ class SysIdRoutineTest : public ::testing::Test {
   frc2::CommandPtr m_dynamicReverse{
       m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
 
+  frc2::sysid::SysIdRoutine m_emptySysidRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr},
+      frc2::sysid::Mechanism{[this](units::volt_t driveVoltage) {}, nullptr,
+                             &m_subsystem}};
+
+  frc2::CommandPtr m_emptyRoutineForward{
+      m_emptySysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+
   void RunCommand(frc2::CommandPtr command) {
     command.get()->Initialize();
     command.get()->Execute();
@@ -168,3 +177,8 @@ TEST_F(SysIdRoutineTest, OutputCorrectVoltage) {
   currentStateList.clear();
   sentVoltages.clear();
 }
+
+TEST_F(SysIdRoutineTest, EmptyLogFunc) {
+  RunCommand(std::move(m_emptyRoutineForward));
+  SUCCEED();
+}