Add callback handlers for LiveWindow (#2053)

Fixes #2223

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -24,6 +24,7 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -35,7 +36,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
  * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
  */
 @SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"})
-public final class CommandScheduler implements Sendable {
+public final class CommandScheduler implements Sendable, AutoCloseable {
   /**
    * The Singleton Instance.
    */
@@ -86,6 +87,20 @@ public final class CommandScheduler implements Sendable {
   CommandScheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      cancelAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -9,6 +9,7 @@
 
 #include <frc/RobotState.h>
 #include <frc/WPIErrors.h>
+#include <frc/livewindow/LiveWindow.h>
 #include <frc/smartdashboard/SendableBuilder.h>
 #include <frc/smartdashboard/SendableRegistry.h>
 #include <hal/FRCUsageReporting.h>
@@ -67,9 +68,20 @@ CommandScheduler::CommandScheduler() : m_impl(new Impl) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand2_Scheduler);
   frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->Disable();
+    this->CancelAll();
+  };
+  scheduler->disabled = [this] { this->Enable(); };
 }
 
-CommandScheduler::~CommandScheduler() {}
+CommandScheduler::~CommandScheduler() {
+  frc::SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 CommandScheduler& CommandScheduler::GetInstance() {
   static CommandScheduler scheduler;

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java

@@ -17,6 +17,7 @@ import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
@@ -95,11 +96,20 @@ public final class Scheduler implements Sendable, AutoCloseable {
   private Scheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
+    LiveWindow.setEnabledListener(() -> {
+      disable();
+      removeAll();
+    });
+    LiveWindow.setDisabledListener(() -> {
+      enable();
+    });
   }
 
   @Override
   public void close() {
     SendableRegistry.remove(this);
+    LiveWindow.setEnabledListener(null);
+    LiveWindow.setDisabledListener(null);
   }
 
   /**

wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp

@@ -20,6 +20,7 @@
 #include "frc/buttons/ButtonScheduler.h"
 #include "frc/commands/Command.h"
 #include "frc/commands/Subsystem.h"
+#include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -198,9 +199,20 @@ Scheduler::Scheduler() : m_impl(new Impl) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand_Scheduler);
   SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = [this] {
+    this->SetEnabled(false);
+    this->RemoveAll();
+  };
+  scheduler->disabled = [this] { this->SetEnabled(true); };
 }
 
-Scheduler::~Scheduler() {}
+Scheduler::~Scheduler() {
+  SendableRegistry::GetInstance().Remove(this);
+  auto scheduler = frc::LiveWindow::GetInstance();
+  scheduler->enabled = nullptr;
+  scheduler->disabled = nullptr;
+}
 
 void Scheduler::Impl::Remove(Command* command) {
   if (!commands.erase(command)) return;

wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp

@@ -104,10 +104,12 @@ void LiveWindow::SetEnabled(bool enabled) {
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
+    this->enabled();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
+    this->disabled();
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }

wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 
 namespace frc {
@@ -22,6 +23,9 @@ class LiveWindow {
   LiveWindow(const LiveWindow&) = delete;
   LiveWindow& operator=(const LiveWindow&) = delete;
 
+  std::function<void()> enabled;
+  std::function<void()> disabled;
+
   /**
    * Get an instance of the LiveWindow main class.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java

@@ -33,6 +33,9 @@ public class LiveWindow {
   private static boolean liveWindowEnabled;
   private static boolean telemetryEnabled = true;
 
+  private static Runnable enabledListener;
+  private static Runnable disabledListener;
+
   private static Component getOrAdd(Sendable sendable) {
     Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
     if (data == null) {
@@ -46,6 +49,14 @@ public class LiveWindow {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
+  public static synchronized void setEnabledListener(Runnable runnable) {
+    enabledListener = runnable;
+  }
+
+  public static synchronized void setDisabledListener(Runnable runnable) {
+    disabledListener = runnable;
+  }
+
   public static synchronized boolean isEnabled() {
     return liveWindowEnabled;
   }
@@ -65,11 +76,17 @@ public class LiveWindow {
       updateValues(); // Force table generation now to make sure everything is defined
       if (enabled) {
         System.out.println("Starting live window mode.");
+        if (enabledListener != null) {
+          enabledListener.run();
+        }
       } else {
         System.out.println("stopping live window mode.");
         SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
           cbdata.builder.stopLiveWindowMode();
         });
+        if (disabledListener != null) {
+          disabledListener.run();
+        }
       }
       enabledEntry.setBoolean(enabled);
     }