[wpilib] fix opmode reset and callback handling

design-docs/opmodes.md

@@ -215,8 +215,8 @@ The lifecycle of an opmode is:
 - When operator selects opmode on DS, a new opmode object is constructed
 - While selected and disabled, `disabledPeriodic()` is called
 - On disabled → enabled transition, `start()` is called once
-- While enabled, `periodic()` is called at `OpModeRobot#getPeriod()`, and additional callbacks from `getCallbacks()` are run at their own configured rates
-- If robot disables or a different opmode is selected while enabled, `end()` is called then `close()` is called (Java), or the object is destroyed (C++/Python); the object is not reused
+- While enabled, `periodic()` is called at `OpModeRobot#getPeriod()`, and additional callbacks from `getCallbacks()` are run at their own configured rates (note: callbacks from `getCallbacks()` are registered immediately when the OpMode is constructed and begin executing as soon as they are registered; to restrict execution to only when enabled, callbacks should include an enabled check)
+- If robot disables or a different opmode is selected while enabled, `end()` is called then `close()` is called (Java), or the object is destroyed (C++/Python); the object is not reused. Note: selecting a different opmode while enabled automatically disables the robot first.
 - If a different opmode is selected while disabled, only `close()` is called (Java), or the object is destroyed (C++); the object is not reused
 
 Following `close()` being called (Java)/the opmode being destroyed (C++), a *new* opmode object is constructed based on the DS teleop/auto/utility/match selector and selected opmode.  In teleop/auto/utility, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again.  In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed.  Thus only zero or one opmode objects will ever be "alive" at any given time.
@@ -258,6 +258,7 @@ public abstract class PeriodicOpMode implements OpMode {
   public Set<PeriodicPriorityQueue.Callback> getCallbacks() {...}
 
   // additional periodic callbacks with custom rates/offsets
+  // callbacks are registered immediately and begin executing as soon as registered
   public final void addPeriodic(Runnable callback, double period) {...}
   public final void addPeriodic(Runnable callback, double period, double offset) {...}
 }

wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp

@@ -63,55 +63,45 @@ void OpModeRobotBase::LoopFunc() {
   m_watchdog.Reset();
   const bool enabled = word.IsEnabled();
   int64_t modeId = word.IsDSAttached() ? word.GetOpModeId() : 0;
+  bool calledOpModeDisabledPeriodicThisIteration = false;
 
   if (!m_calledDriverStationConnected && word.IsDSAttached()) {
     m_calledDriverStationConnected = true;
     DriverStationConnected();
     m_watchdog.AddEpoch("DriverStationConnected()");
   }
 
-  // Handle opmode changes
-  if (modeId != m_lastModeId) {
-    // Clean up current opmode
-    if (m_currentOpMode) {
-      // Remove opmode callbacks
-      if (m_opmodePeriodic) {
-        m_callbacks.Remove(*m_opmodePeriodic);
-        m_opmodePeriodic.reset();
-      }
-      for (auto& cb : m_activeOpModeCallbacks) {
-        m_callbacks.Remove(cb);
-      }
-      m_activeOpModeCallbacks.clear();
-      m_currentOpMode.reset();
-    }
+  // Handle OpMode changes
+  if (modeId != m_lastModeId && m_currentOpMode) {
+    EndCurrentOpMode();
+  }
 
-    // Set up new opmode
-    if (modeId != 0) {
-      auto data = m_opModes.lookup(modeId);
-      if (data.factory) {
-        // Instantiate the new opmode
-        fmt::print("********** Starting OpMode {} **********\n", data.name);
-        m_currentOpMode = data.factory();
-        if (m_currentOpMode) {
-          // Ensure disabledPeriodic is called at least once
-          m_currentOpMode->DisabledPeriodic();
-          m_watchdog.AddEpoch("OpMode::DisabledPeriodic()");
-          // Register the opmode's periodic callbacks
-          m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{
-              [op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime,
-              m_period};
-          m_callbacks.Add(*m_opmodePeriodic);
-          m_activeOpModeCallbacks = m_currentOpMode->GetCallbacks();
-          for (auto& cb : m_activeOpModeCallbacks) {
-            m_callbacks.Add(cb);
-          }
+  // Set up new opmode
+  if (modeId != 0 && !m_currentOpMode) {
+    auto data = m_opModes.lookup(modeId);
+    if (data.factory) {
+      // Instantiate the new opmode
+      m_currentOpModeName = data.name;
+      fmt::print("********** Creating OpMode {} **********\n",
+                 m_currentOpModeName);
+      m_currentOpMode = data.factory();
+      if (m_currentOpMode) {
+        // Register the opmode's additional periodic callbacks immediately on
+        // creation
+        m_activeOpModeCallbacks = m_currentOpMode->GetCallbacks();
+        for (auto& cb : m_activeOpModeCallbacks) {
+          m_callbacks.Add(cb);
         }
-      } else {
-        WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId);
+
+        // Call DisabledPeriodic immediately for newly created OpMode when
+        // disabled
+        m_currentOpMode->DisabledPeriodic();
+        m_watchdog.AddEpoch("OpMode::DisabledPeriodic()");
+        calledOpModeDisabledPeriodicThisIteration = true;
       }
+    } else {
+      WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId);
     }
-    m_lastModeId = modeId;
   }
 
   // Handle enabled state changes
@@ -121,16 +111,12 @@ void OpModeRobotBase::LoopFunc() {
       // Transitioning to enabled
       DisabledExit();
       m_watchdog.AddEpoch("DisabledExit()");
-      if (m_currentOpMode) {
-        m_currentOpMode->Start();
-        m_watchdog.AddEpoch("OpMode::Start()");
-      }
     } else {
-      // Transitioning to disabled
-      if (m_currentOpMode && m_lastEnabledState) {
-        // Was enabled, now disabled
-        m_currentOpMode->End();
-        m_watchdog.AddEpoch("OpMode::End()");
+      // Transitioning to disabled. Only tear down an opmode that was actually
+      // running; a freshly selected opmode entering its disabled phase must
+      // persist so it can be started on the next enable.
+      if (m_currentOpMode && m_opmodePeriodic) {
+        EndCurrentOpMode();
       }
       DisabledInit();
       m_watchdog.AddEpoch("DisabledInit()");
@@ -139,6 +125,13 @@ void OpModeRobotBase::LoopFunc() {
     m_lastEnabledState = enabled;
   }
 
+  // Start the opmode if enabled and not already started. This single check
+  // covers both the disabled->enabled transition and an opmode constructed
+  // while the robot is already enabled.
+  if (enabled && m_currentOpMode && !m_opmodePeriodic) {
+    StartCurrentOpMode();
+  }
+
   // Call periodic functions based on current state
   if (!enabled) {
     // Only call DisabledPeriodic if we didn't just call DisabledInit
@@ -147,13 +140,16 @@ void OpModeRobotBase::LoopFunc() {
       m_watchdog.AddEpoch("DisabledPeriodic()");
     }
 
-    // Call opmode DisabledPeriodic if we have one
-    if (m_currentOpMode) {
+    // Call opmode DisabledPeriodic if we have one and haven't called it already
+    // this iteration
+    if (m_currentOpMode && !calledOpModeDisabledPeriodicThisIteration) {
       m_currentOpMode->DisabledPeriodic();
       m_watchdog.AddEpoch("OpMode::DisabledPeriodic()");
     }
   }
 
+  m_lastModeId = modeId;
+
   // Call NonePeriodic when no opmode is selected
   if (modeId == 0) {
     NonePeriodic();
@@ -249,3 +245,54 @@ void OpModeRobotBase::ClearOpModes() {
   RobotState::ClearOpModes();
   m_opModes.clear();
 }
+
+void OpModeRobotBase::StartCurrentOpMode() {
+  if (!m_currentOpMode || m_opmodePeriodic) {
+    return;
+  }
+
+  fmt::print("********** Starting OpMode {} **********\n", m_currentOpModeName);
+
+  // Register the main opmode periodic callback. Capture a weak_ptr so a queued
+  // callback can never resurrect or outlive a destroyed opmode.
+  m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{
+      [op = std::weak_ptr<OpMode>{m_currentOpMode}] {
+        if (auto shared_op = op.lock()) {
+          shared_op->Periodic();
+        }
+      },
+      m_startTime, m_period};
+  m_callbacks.Add(*m_opmodePeriodic);
+
+  m_currentOpMode->Start();
+  m_watchdog.AddEpoch("OpMode::Start()");
+}
+
+void OpModeRobotBase::EndCurrentOpMode() {
+  if (!m_currentOpMode) {
+    return;
+  }
+
+  // If the opmode was started, end it and remove its main periodic callback.
+  if (m_opmodePeriodic) {
+    fmt::print("********** Ending OpMode {} **********\n", m_currentOpModeName);
+
+    m_currentOpMode->End();
+    m_watchdog.AddEpoch("OpMode::End()");
+
+    m_callbacks.Remove(*m_opmodePeriodic);
+    m_opmodePeriodic.reset();
+  }
+
+  // The additional GetCallbacks() callbacks are registered immediately on
+  // construction (even while disabled), so always remove them regardless of
+  // whether the opmode was started.
+  for (auto& cb : m_activeOpModeCallbacks) {
+    m_callbacks.Remove(cb);
+  }
+  m_activeOpModeCallbacks.clear();
+
+  // Regardless of whether opmode was started, destroy it
+  fmt::print("********** Closing OpMode {} **********\n", m_currentOpModeName);
+  m_currentOpMode.reset();
+}

wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp

@@ -4,6 +4,8 @@
 
 #include "wpi/internal/PeriodicPriorityQueue.hpp"
 
+#include <atomic>
+#include <cstdint>
 #include <utility>
 
 #include "wpi/hal/Notifier.h"
@@ -13,6 +15,12 @@
 
 using namespace wpi::internal;
 
+namespace {
+// Monotonic source of stable callback identities. Shared across all callbacks
+// so each fresh construction gets a unique id; copies preserve their id.
+std::atomic<uint64_t> gNextCallbackId{1};
+}  // namespace
+
 PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
                                           std::chrono::microseconds startTime,
                                           std::chrono::microseconds period,
@@ -23,7 +31,8 @@ PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
           startTime + offset + period +
           (std::chrono::microseconds{RobotController::GetMonotonicTime()} -
            startTime) /
-              period * period) {}
+              period * period),
+      id{gNextCallbackId.fetch_add(1, std::memory_order_relaxed)} {}
 
 PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
                                           std::chrono::microseconds startTime,

wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp

@@ -138,6 +138,9 @@ class OpModeRobotBase : public RobotBase {
   /**
    * Add a callback to run at a specific period.
    *
+   * This callback will be registered with the framework immediately when this
+   * method is called and will begin executing as soon as it is registered.
+   *
    * @param callback The callback to run.
    * @param period The period at which to run the callback.
    */
@@ -217,6 +220,18 @@ class OpModeRobotBase : public RobotBase {
    */
   void LoopFunc();
 
+  /**
+   * Starts the current OpMode, registering its periodic callback and calling
+   * Start(). Does nothing if there is no current OpMode or it is already
+   * started.
+   */
+  void StartCurrentOpMode();
+
+  /**
+   * Ends the current OpMode, cleaning up callbacks and resetting state.
+   */
+  void EndCurrentOpMode();
+
  private:
   struct OpModeData {
     std::string name;
@@ -238,6 +253,7 @@ class OpModeRobotBase : public RobotBase {
   bool m_calledDriverStationConnected = false;
   bool m_lastEnabledState = false;
   std::shared_ptr<OpMode> m_currentOpMode;
+  std::string m_currentOpModeName;
   std::vector<wpi::internal::PeriodicPriorityQueue::Callback>
       m_activeOpModeCallbacks;
   std::optional<wpi::internal::PeriodicPriorityQueue::Callback>

wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <chrono>
+#include <cstdint>
 #include <functional>
 #include <vector>
 
@@ -47,6 +48,12 @@ class PeriodicPriorityQueue {
     /** The next scheduled execution time in FPGA timestamp microseconds. */
     std::chrono::microseconds expirationTime;
 
+    /**
+     * The unique id for this callback to allow callbacks to be tracked and
+     * removed from the queue throughout robot operation.
+     */
+    uint64_t id;
+
     /**
      * Construct a callback container.
      *
@@ -84,9 +91,7 @@ class PeriodicPriorityQueue {
       return expirationTime > rhs.expirationTime;
     }
 
-    bool operator==(const Callback& rhs) const {
-      return expirationTime == rhs.expirationTime;
-    }
+    bool operator==(const Callback& rhs) const { return id == rhs.id; }
   };
 
   /**

wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp

@@ -69,6 +69,12 @@ class OpMode {
    * framework at their scheduled times, in addition to the primary Periodic()
    * callback.
    *
+   * <p><b>Registration timing</b>: The callbacks returned by this method are
+   * registered with the framework immediately when the OpMode is constructed
+   * and begin executing as soon as they are registered. To restrict execution
+   * to only when the robot is enabled, callbacks should include an
+   * if (RobotState.isEnabled()) check at the beginning.
+   *
    * @return A vector of custom callbacks to execute, or an empty vector if no
    *         custom callbacks are needed. The default implementation returns an
    *         empty vector.

wpilibc/src/main/python/semiwrap/OpModeRobot.yml

@@ -29,6 +29,10 @@ classes:
       AddPeriodic:
       GetLoopStartTime:
       LoopFunc:
+      StartCurrentOpMode:
+        ignore: true
+      EndCurrentOpMode:
+        ignore: true
     attributes:
       DEFAULT_PERIOD:
   wpi::OpModeRobot:

wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml

@@ -22,6 +22,7 @@ classes:
       func:
       period:
       expirationTime:
+      id:
     methods:
       Callback:
         overloads: