From 80d5c1995f5a74dcdf37b48f916db8db2003c22e Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Mon, 11 May 2026 16:36:31 -0400 Subject: [PATCH 01/10] Refactor opmode management to streamline callback handling and reset logic Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 89 +++++++++++-------- .../org/wpilib/framework/OpModeRobot.java | 76 +++++++++------- 2 files changed, 94 insertions(+), 71 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index e90c1c7672b..7d01cca4c36 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -71,48 +71,39 @@ void OpModeRobotBase::LoopFunc() { } // 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(); + if (modeId != m_lastModeId && m_currentOpMode) { + // Remove current 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(); - // 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); - } - } - } else { - WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); + // Reset current opmode + m_currentOpMode->End(); + m_currentOpMode.reset(); + } + + // Set up new opmode + if (modeId != 0 && !m_currentOpMode) { + 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()"); } + } else { + WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); } - m_lastModeId = modeId; } + m_lastModeId = modeId; // Handle enabled state changes bool justCalledDisabledInit = false; @@ -122,6 +113,17 @@ void OpModeRobotBase::LoopFunc() { DisabledExit(); m_watchdog.AddEpoch("DisabledExit()"); if (m_currentOpMode) { + // 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); + } + + // Start the opmode m_currentOpMode->Start(); m_watchdog.AddEpoch("OpMode::Start()"); } @@ -131,6 +133,19 @@ void OpModeRobotBase::LoopFunc() { // Was enabled, now disabled m_currentOpMode->End(); m_watchdog.AddEpoch("OpMode::End()"); + + // Remove opmode callbacks + if (m_opmodePeriodic) { + m_callbacks.Remove(*m_opmodePeriodic); + m_opmodePeriodic.reset(); + } + for (auto& cb : m_activeOpModeCallbacks) { + m_callbacks.Remove(cb); + } + + // Reset opmode + m_activeOpModeCallbacks.clear(); + m_currentOpMode.reset(); } DisabledInit(); m_watchdog.AddEpoch("DisabledInit()"); diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index dd84f80e279..f71d87c7020 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -648,7 +648,7 @@ private void loopFunc() { // Get current enabled state and opmode DriverStationBackend.refreshControlWordFromCache(m_word); m_watchdog.reset(); - boolean enabled = m_word.isEnabled(); + final boolean enabled = m_word.isEnabled(); long modeId = m_word.isDSAttached() ? m_word.getOpModeId() : 0; if (!m_calledDriverStationConnected && m_word.isDSAttached()) { @@ -658,41 +658,35 @@ private void loopFunc() { } // Handle opmode changes - if (modeId != m_lastModeId) { - // Clean up current opmode - if (m_currentOpMode != null) { - // Remove opmode callbacks - m_callbacks.remove(m_currentOpModePeriodic); - m_callbacks.removeAll(m_activeOpModeCallbacks); - m_activeOpModeCallbacks.clear(); - m_currentOpMode.end(); - m_currentOpMode.close(); - m_currentOpMode = null; - } - - // Set up new opmode - if (modeId != 0) { - OpModeFactory factory = m_opModes.get(modeId); - if (factory != null) { - // Instantiate the new opmode - System.out.println("********** Starting OpMode " + factory.name() + " **********"); - m_currentOpMode = factory.supplier().get(); - if (m_currentOpMode != null) { - // Ensure disabledPeriodic is called at least once - m_currentOpMode.disabledPeriodic(); - m_watchdog.addEpoch("opMode.disabledPeriodic()"); - // Register the opmode's periodic callbacks - m_currentOpModePeriodic = - m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); - m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); - m_callbacks.addAll(m_activeOpModeCallbacks); - } - } else { - DriverStationErrors.reportError("No OpMode found for mode " + modeId, false); + if (modeId != m_lastModeId && m_currentOpMode != null) { + // Remove current opmode callbacks + m_callbacks.remove(m_currentOpModePeriodic); + m_callbacks.removeAll(m_activeOpModeCallbacks); + m_activeOpModeCallbacks.clear(); + + // Reset current opmode + m_currentOpMode.end(); + m_currentOpMode.close(); + m_currentOpMode = null; + } + + // Set up new opmode + if (modeId != 0 && m_currentOpMode == null) { + OpModeFactory factory = m_opModes.get(modeId); + if (factory != null) { + // Instantiate the new opmode + System.out.println("********** Starting OpMode " + factory.name() + " **********"); + m_currentOpMode = factory.supplier().get(); + if (m_currentOpMode != null) { + // Ensure disabledPeriodic is called at least once + m_currentOpMode.disabledPeriodic(); + m_watchdog.addEpoch("opMode.disabledPeriodic()"); } + } else { + DriverStationErrors.reportError("No OpMode found for mode " + modeId, false); } - m_lastModeId = modeId; } + m_lastModeId = modeId; // Handle enabled state changes boolean justCalledDisabledInit = false; @@ -702,15 +696,29 @@ private void loopFunc() { disabledExit(); m_watchdog.addEpoch("disabledExit()"); if (m_currentOpMode != null) { + // Register the opmode's periodic callbacks + m_currentOpModePeriodic = + m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); + m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); + m_callbacks.addAll(m_activeOpModeCallbacks); + + // Start the opmode m_currentOpMode.start(); m_watchdog.addEpoch("opMode.start()"); } } else { // Transitioning to disabled - if (m_currentOpMode != null && m_lastEnabledState) { + if (m_currentOpMode != null) { // Was enabled, now disabled m_currentOpMode.end(); m_watchdog.addEpoch("opMode.end()"); + + // Remove opmode callbacks + m_callbacks.remove(m_currentOpModePeriodic); + m_callbacks.removeAll(m_activeOpModeCallbacks); + m_activeOpModeCallbacks.clear(); + m_currentOpMode.close(); + m_currentOpMode = null; } disabledInit(); m_watchdog.addEpoch("disabledInit()"); From 7877a8764efcc4d5e7db24db78b3ae0cc2ae14bd Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Mon, 11 May 2026 21:37:33 -0400 Subject: [PATCH 02/10] add lifecycle tests Signed-off-by: Zach Harel --- .../cpp/framework/OpModeLifecycleTest.cpp | 189 +++++++++++ .../wpilib/framework/OpModeLifecycleTest.java | 296 ++++++++++++++++++ 2 files changed, 485 insertions(+) create mode 100644 wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp create mode 100644 wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java diff --git a/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp new file mode 100644 index 00000000000..b408c0d8d3e --- /dev/null +++ b/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp @@ -0,0 +1,189 @@ +// 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 +#include + +#include + +#include "wpi/framework/OpModeRobot.hpp" +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/simulation/DriverStationSim.hpp" +#include "wpi/simulation/SimHooks.hpp" +#include "wpi/util/string.hpp" + +using namespace wpi::units; + +namespace { +struct Counts { + std::atomic constructed{0}; + std::atomic disabledPeriodic{0}; + std::atomic start{0}; + std::atomic periodic{0}; + std::atomic end{0}; + std::atomic destructed{0}; +}; + +class LifecycleOpMode : public wpi::OpMode { + public: + explicit LifecycleOpMode(Counts& counts) : m_counts(counts) { + m_counts.constructed++; + } + + ~LifecycleOpMode() override { m_counts.destructed++; } + + void DisabledPeriodic() override { m_counts.disabledPeriodic++; } + void Start() override { m_counts.start++; } + void Periodic() override { m_counts.periodic++; } + void End() override { m_counts.end++; } + + private: + Counts& m_counts; +}; + +class LifecycleRobot : public wpi::OpModeRobot { + public: + LifecycleRobot() = default; +}; + +class OpModeLifecycleTest : public ::testing::Test { + protected: + void SetUp() override { + wpi::sim::PauseTiming(); + wpi::sim::SetProgramStarted(false); + wpi::sim::DriverStationSim::ResetData(); + wpi::sim::DriverStationSim::SetDsAttached(true); + wpi::sim::DriverStationSim::SetEnabled(false); + } + + void TearDown() override { + wpi::sim::ResumeTiming(); + } + + static int64_t MakeOpModeId(wpi::RobotMode mode, std::string_view name) { + return HAL_MakeOpModeId(static_cast(mode), std::hash{}(name)); + } +}; + +TEST_F(OpModeLifecycleTest, EnabledTransition) { + Counts counts; + LifecycleRobot robot; + robot.AddOpModeFactory([&] { return std::make_unique(counts); }, + wpi::RobotMode::TELEOPERATED, "TestOpMode"); + robot.PublishOpModes(); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + // Ensure DS is attached after program starts + wpi::sim::DriverStationSim::SetDsAttached(true); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); // Let the DS attached state propagate + + // 1. Selected, but disabled + wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "TestOpMode")); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + EXPECT_EQ(counts.constructed.load(), 1u); + EXPECT_EQ(counts.disabledPeriodic.load(), 2u); + + // 2. Transition to enabled + wpi::sim::DriverStationSim::SetEnabled(true); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + EXPECT_EQ(counts.start.load(), 1u); + EXPECT_EQ(counts.periodic.load(), 1u); + + // 3. Transition to disabled + wpi::sim::DriverStationSim::SetEnabled(false); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + EXPECT_EQ(counts.end.load(), 1u); + EXPECT_EQ(counts.destructed.load(), 1u); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(OpModeLifecycleTest, OpModeChangeWhileEnabled) { + Counts counts1; + Counts counts2; + LifecycleRobot robot; + robot.AddOpModeFactory([&] { return std::make_unique(counts1); }, + wpi::RobotMode::TELEOPERATED, "OpMode1"); + robot.AddOpModeFactory([&] { return std::make_unique(counts2); }, + wpi::RobotMode::TELEOPERATED, "OpMode2"); + robot.PublishOpModes(); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + // Ensure DS is attached after program starts + wpi::sim::DriverStationSim::SetDsAttached(true); + wpi::sim::DriverStationSim::NotifyNewData(); + + // 1. Select OpMode1 and enable + wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); + wpi::sim::DriverStationSim::SetEnabled(true); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + EXPECT_EQ(counts1.constructed.load(), 1u); + EXPECT_EQ(counts1.start.load(), 1u); + EXPECT_EQ(counts1.periodic.load(), 1u); + + // 2. Change to OpMode2 while enabled + wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + // OpMode1 should be ended and destructed + EXPECT_EQ(counts1.end.load(), 1u); + EXPECT_EQ(counts1.destructed.load(), 1u); + // OpMode2 should be started + EXPECT_EQ(counts2.constructed.load(), 1u); + EXPECT_EQ(counts2.start.load(), 1u); + EXPECT_EQ(counts2.periodic.load(), 1u); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(OpModeLifecycleTest, OpModeChangeWhileDisabled) { + Counts counts1; + Counts counts2; + LifecycleRobot robot; + robot.AddOpModeFactory([&] { return std::make_unique(counts1); }, + wpi::RobotMode::TELEOPERATED, "OpMode1"); + robot.AddOpModeFactory([&] { return std::make_unique(counts2); }, + wpi::RobotMode::TELEOPERATED, "OpMode2"); + robot.PublishOpModes(); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + // Ensure DS is attached after program starts + wpi::sim::DriverStationSim::SetDsAttached(true); + + // 1. Select OpMode1 while disabled + wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + EXPECT_EQ(counts1.constructed.load(), 1u); + EXPECT_EQ(counts1.disabledPeriodic.load(), 2u); + + // 2. Change to OpMode2 while disabled + wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(20_ms); + // OpMode1 should be destructed, but NOT ended + EXPECT_EQ(counts1.destructed.load(), 1u); + EXPECT_EQ(counts1.end.load(), 0u); + // OpMode2 should be selected + EXPECT_EQ(counts2.constructed.load(), 1u); + EXPECT_EQ(counts2.disabledPeriodic.load(), 2u); + + robot.EndCompetition(); + robotThread.join(); +} + +} // namespace diff --git a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java new file mode 100644 index 00000000000..344e491a01f --- /dev/null +++ b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java @@ -0,0 +1,296 @@ +// 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. + +package org.wpilib.framework; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.parallel.ResourceLock; +import org.wpilib.driverstation.internal.DriverStationBackend; +import org.wpilib.hardware.hal.OpModeOption; +import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.opmode.OpMode; +import org.wpilib.simulation.DriverStationSim; +import org.wpilib.simulation.SimHooks; + +@ResourceLock("timing") +class OpModeLifecycleTest { + private static final double kPeriod = 0.02; + + private static long makeOpModeId(RobotMode mode, String name) { + return OpModeOption.makeId(mode, name.hashCode()); + } + + static class LifecycleOpMode implements OpMode { + public final AtomicInteger m_constructedCount; + public final AtomicInteger m_disabledPeriodicCount; + public final AtomicInteger m_startCount; + public final AtomicInteger m_periodicCount; + public final AtomicInteger m_endCount; + public final AtomicInteger m_closeCount; + + LifecycleOpMode( + AtomicInteger constructedCount, + AtomicInteger disabledPeriodicCount, + AtomicInteger startCount, + AtomicInteger periodicCount, + AtomicInteger endCount, + AtomicInteger closeCount) { + m_constructedCount = constructedCount; + m_disabledPeriodicCount = disabledPeriodicCount; + m_startCount = startCount; + m_periodicCount = periodicCount; + m_endCount = endCount; + m_closeCount = closeCount; + m_constructedCount.incrementAndGet(); + } + + @Override + public void disabledPeriodic() { + m_disabledPeriodicCount.incrementAndGet(); + } + + @Override + public void start() { + m_startCount.incrementAndGet(); + } + + @Override + public void periodic() { + m_periodicCount.incrementAndGet(); + } + + @Override + public void end() { + m_endCount.incrementAndGet(); + } + + @Override + public void close() { + m_closeCount.incrementAndGet(); + } + } + + static class LifecycleRobot extends OpModeRobot { + LifecycleRobot() { + super(); + } + } + + @BeforeEach + void setUp() { + SimHooks.pauseTiming(); + SimHooks.setProgramStarted(false); + DriverStationSim.resetData(); + DriverStationSim.setDsAttached(true); + DriverStationSim.setEnabled(false); + } + + @AfterEach + void tearDown() { + DriverStationBackend.clearOpModes(); + SimHooks.resumeTiming(); + } + + @Test + void testLifecycleEnabledTransition() throws InterruptedException { + AtomicInteger constructedCount = new AtomicInteger(0); + AtomicInteger disabledPeriodicCount = new AtomicInteger(0); + AtomicInteger startCount = new AtomicInteger(0); + AtomicInteger periodicCount = new AtomicInteger(0); + AtomicInteger endCount = new AtomicInteger(0); + AtomicInteger closeCount = new AtomicInteger(0); + + LifecycleRobot robot = new LifecycleRobot(); + robot.addOpModeFactory( + () -> + new LifecycleOpMode( + constructedCount, + disabledPeriodicCount, + startCount, + periodicCount, + endCount, + closeCount), + RobotMode.TELEOPERATED, + "TestOpMode"); + robot.publishOpModes(); + var options = DriverStationSim.getOpModeOptions(); + var id = makeOpModeId(RobotMode.TELEOPERATED, "TestOpMode"); + assertEquals(1, options.length); + assertEquals(id, options[0].id); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // 1. Selected, but disabled + DriverStationSim.setOpMode(id); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + assertEquals(1, constructedCount.get()); + assertEquals(2, disabledPeriodicCount.get()); + + // 2. Transition to enabled + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + assertEquals(1, startCount.get()); + assertEquals(1, periodicCount.get()); + + // 3. Transition to disabled + DriverStationSim.setEnabled(false); + DriverStationSim.notifyNewData(); + + SimHooks.stepTiming(kPeriod); + assertEquals(1, endCount.get()); + assertEquals(1, closeCount.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } + + @Test + void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { + AtomicInteger constructedCount1 = new AtomicInteger(0); + AtomicInteger disabledPeriodicCount1 = new AtomicInteger(0); + AtomicInteger startCount1 = new AtomicInteger(0); + AtomicInteger periodicCount1 = new AtomicInteger(0); + AtomicInteger endCount1 = new AtomicInteger(0); + AtomicInteger closeCount1 = new AtomicInteger(0); + + AtomicInteger constructedCount2 = new AtomicInteger(0); + AtomicInteger disabledPeriodicCount2 = new AtomicInteger(0); + AtomicInteger startCount2 = new AtomicInteger(0); + AtomicInteger periodicCount2 = new AtomicInteger(0); + AtomicInteger endCount2 = new AtomicInteger(0); + AtomicInteger closeCount2 = new AtomicInteger(0); + + LifecycleRobot robot = new LifecycleRobot(); + robot.addOpModeFactory( + () -> + new LifecycleOpMode( + constructedCount1, + disabledPeriodicCount1, + startCount1, + periodicCount1, + endCount1, + closeCount1), + RobotMode.TELEOPERATED, + "OpMode1"); + robot.addOpModeFactory( + () -> + new LifecycleOpMode( + constructedCount2, + disabledPeriodicCount2, + startCount2, + periodicCount2, + endCount2, + closeCount2), + RobotMode.TELEOPERATED, + "OpMode2"); + robot.publishOpModes(); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // 1. Select OpMode1 and enable + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode1")); + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + assertEquals(1, constructedCount1.get()); + assertEquals(1, startCount1.get()); + assertEquals(1, periodicCount1.get()); + + // 2. Change to OpMode2 while enabled + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + // OpMode1 should be ended and closed + assertEquals(1, endCount1.get()); + assertEquals(1, closeCount1.get()); + // OpMode2 should be started + assertEquals(1, constructedCount2.get()); + assertEquals(1, startCount2.get()); + assertEquals(1, periodicCount2.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } + + @Test + void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { + AtomicInteger constructedCount1 = new AtomicInteger(0); + AtomicInteger disabledPeriodicCount1 = new AtomicInteger(0); + AtomicInteger startCount1 = new AtomicInteger(0); + AtomicInteger periodicCount1 = new AtomicInteger(0); + AtomicInteger endCount1 = new AtomicInteger(0); + AtomicInteger closeCount1 = new AtomicInteger(0); + + AtomicInteger constructedCount2 = new AtomicInteger(0); + AtomicInteger disabledPeriodicCount2 = new AtomicInteger(0); + AtomicInteger startCount2 = new AtomicInteger(0); + AtomicInteger periodicCount2 = new AtomicInteger(0); + AtomicInteger endCount2 = new AtomicInteger(0); + AtomicInteger closeCount2 = new AtomicInteger(0); + + LifecycleRobot robot = new LifecycleRobot(); + robot.addOpModeFactory( + () -> + new LifecycleOpMode( + constructedCount1, + disabledPeriodicCount1, + startCount1, + periodicCount1, + endCount1, + closeCount1), + RobotMode.TELEOPERATED, + "OpMode1"); + robot.addOpModeFactory( + () -> + new LifecycleOpMode( + constructedCount2, + disabledPeriodicCount2, + startCount2, + periodicCount2, + endCount2, + closeCount2), + RobotMode.TELEOPERATED, + "OpMode2"); + robot.publishOpModes(); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // 1. Select OpMode1 while disabled + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode1")); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + assertEquals(1, constructedCount1.get()); + assertEquals(2, disabledPeriodicCount1.get()); + + // 2. Change to OpMode2 while disabled + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(kPeriod); + // OpMode1 should be closed, but NOT ended (since it never started) + assertEquals(1, closeCount1.get()); + assertEquals(0, endCount1.get()); + // OpMode2 should be selected + assertEquals(1, constructedCount2.get()); + assertEquals(2, disabledPeriodicCount2.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } +} From f8e69176ef359bbef0f4789ba61d730796791348 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 12 May 2026 22:28:31 -0400 Subject: [PATCH 03/10] Refactor OpMode management to improve state handling and cleanup logic Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 128 ++++++++++++------ .../include/wpi/framework/OpModeRobot.hpp | 8 ++ .../src/main/python/semiwrap/OpModeRobot.yml | 1 + .../org/wpilib/framework/OpModeRobot.java | 90 +++++++----- 4 files changed, 148 insertions(+), 79 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index 7d01cca4c36..f312ece44ce 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -63,6 +63,7 @@ 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; @@ -70,40 +71,56 @@ void OpModeRobotBase::LoopFunc() { m_watchdog.AddEpoch("DriverStationConnected()"); } - // Handle opmode changes + // Handle OpMode changes if (modeId != m_lastModeId && m_currentOpMode) { - // Remove current 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(); - - // Reset current opmode - m_currentOpMode->End(); - m_currentOpMode.reset(); + EndCurrentOpMode(); } // Set up new opmode if (modeId != 0 && !m_currentOpMode) { + fmt::print( + "DEBUG: Looking for OpMode with ID {} in registry with {} entries\n", + modeId, m_opModes.size()); auto data = m_opModes.lookup(modeId); if (data.factory) { // Instantiate the new opmode - fmt::print("********** Starting OpMode {} **********\n", data.name); + fmt::print("********** Creating 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 additional periodic callbacks immediately on + // creation + m_activeOpModeCallbacks = m_currentOpMode->GetCallbacks(); + for (auto& cb : m_activeOpModeCallbacks) { + m_callbacks.Add(cb); + } + + if (!enabled) { + // Call DisabledPeriodic immediately for newly created OpMode when + // disabled + m_currentOpMode->DisabledPeriodic(); + m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); + calledOpModeDisabledPeriodicThisIteration = true; + } else { + // If robot is enabled, start the OpMode immediately + if (!m_opmodePeriodic) { + fmt::print("********** Starting OpMode **********\n"); + // Register the main opmode periodic callback + m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ + [op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime, + m_period}; + m_callbacks.Add(*m_opmodePeriodic); + + m_currentOpMode->Start(); + m_watchdog.AddEpoch("OpMode::Start()"); + } + } } + // Update m_lastModeId immediately to prevent race conditions + m_lastModeId = modeId; } else { WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); } } - m_lastModeId = modeId; // Handle enabled state changes bool justCalledDisabledInit = false; @@ -112,40 +129,23 @@ void OpModeRobotBase::LoopFunc() { // Transitioning to enabled DisabledExit(); m_watchdog.AddEpoch("DisabledExit()"); - if (m_currentOpMode) { - // Register the opmode's periodic callbacks + if (m_currentOpMode && !m_opmodePeriodic) { + // Only start if not already started + // Register the main opmode periodic callback 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); - } // Start the opmode + fmt::print("********** Starting OpMode **********\n"); 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()"); - - // Remove opmode callbacks - if (m_opmodePeriodic) { - m_callbacks.Remove(*m_opmodePeriodic); - m_opmodePeriodic.reset(); - } - for (auto& cb : m_activeOpModeCallbacks) { - m_callbacks.Remove(cb); - } - - // Reset opmode - m_activeOpModeCallbacks.clear(); - m_currentOpMode.reset(); + if (m_currentOpMode) { + EndCurrentOpMode(); } DisabledInit(); m_watchdog.AddEpoch("DisabledInit()"); @@ -162,8 +162,9 @@ 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()"); } @@ -171,6 +172,7 @@ void OpModeRobotBase::LoopFunc() { // Call NonePeriodic when no opmode is selected if (modeId == 0) { + m_lastModeId = modeId; NonePeriodic(); m_watchdog.AddEpoch("NonePeriodic()"); } @@ -235,7 +237,13 @@ void OpModeRobotBase::AddOpModeFactory( int64_t id = RobotState::AddOpMode(mode, name, group, description, textColor, backgroundColor); if (id != 0) { + fmt::print("DEBUG: Registering OpMode '{}' with ID {}\n", name, id); m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; + } else { + fmt::print( + "DEBUG: Failed to register OpMode '{}' - RobotState::AddOpMode " + "returned 0\n", + name); } } @@ -245,7 +253,13 @@ void OpModeRobotBase::AddOpModeFactory(OpModeFactory factory, RobotMode mode, std::string_view description) { int64_t id = RobotState::AddOpMode(mode, name, group, description); if (id != 0) { + fmt::print("DEBUG: Registering OpMode '{}' with ID {}\n", name, id); m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; + } else { + fmt::print( + "DEBUG: Failed to register OpMode '{}' - RobotState::AddOpMode " + "returned 0\n", + name); } } @@ -264,3 +278,29 @@ void OpModeRobotBase::ClearOpModes() { RobotState::ClearOpModes(); m_opModes.clear(); } + +void OpModeRobotBase::EndCurrentOpMode() { + if (!m_currentOpMode) { + return; + } + + // If opmode was started (enabled) + if (m_opmodePeriodic) { + fmt::print("********** Ending OpMode **********\n"); + + m_currentOpMode->End(); + m_watchdog.AddEpoch("OpMode::End()"); + + // Remove opmode callbacks + m_callbacks.Remove(*m_opmodePeriodic); + m_opmodePeriodic.reset(); + 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_currentOpMode.reset(); +} diff --git a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp index de1df00e0a4..172e676c6fb 100644 --- a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp +++ b/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,11 @@ class OpModeRobotBase : public RobotBase { */ void LoopFunc(); + /** + * Ends the current OpMode, cleaning up callbacks and resetting state. + */ + void EndCurrentOpMode(); + private: struct OpModeData { std::string name; diff --git a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml index 20f7f43d58b..c6d316a9249 100644 --- a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml +++ b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml @@ -29,6 +29,7 @@ classes: AddPeriodic: GetLoopStartTime: LoopFunc: + EndCurrentOpMode: attributes: DEFAULT_PERIOD: wpi::OpModeRobot: diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index f71d87c7020..ba8c1a9711f 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -80,6 +80,7 @@ private record OpModeFactory(String name, Supplier supplier) {} private boolean m_calledDriverStationConnected = false; private boolean m_lastEnabledState = false; private OpMode m_currentOpMode; + private String m_currentOpModeName; private Callback m_currentOpModePeriodic; private final Set m_activeOpModeCallbacks = new HashSet<>(); private final Watchdog m_watchdog; @@ -582,6 +583,9 @@ public OpModeRobot(double period) { /** * 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. */ @@ -650,6 +654,7 @@ private void loopFunc() { m_watchdog.reset(); final boolean enabled = m_word.isEnabled(); long modeId = m_word.isDSAttached() ? m_word.getOpModeId() : 0; + boolean calledOpModeDisabledPeriodicThisIteration = false; if (!m_calledDriverStationConnected && m_word.isDSAttached()) { m_calledDriverStationConnected = true; @@ -657,36 +662,32 @@ private void loopFunc() { m_watchdog.addEpoch("driverStationConnected()"); } - // Handle opmode changes - if (modeId != m_lastModeId && m_currentOpMode != null) { - // Remove current opmode callbacks - m_callbacks.remove(m_currentOpModePeriodic); - m_callbacks.removeAll(m_activeOpModeCallbacks); - m_activeOpModeCallbacks.clear(); - - // Reset current opmode - m_currentOpMode.end(); - m_currentOpMode.close(); - m_currentOpMode = null; - } - // Set up new opmode if (modeId != 0 && m_currentOpMode == null) { OpModeFactory factory = m_opModes.get(modeId); if (factory != null) { - // Instantiate the new opmode - System.out.println("********** Starting OpMode " + factory.name() + " **********"); + m_currentOpModeName = factory.name(); + System.out.println("********** Creating OpMode " + m_currentOpModeName + " **********"); m_currentOpMode = factory.supplier().get(); if (m_currentOpMode != null) { - // Ensure disabledPeriodic is called at least once - m_currentOpMode.disabledPeriodic(); - m_watchdog.addEpoch("opMode.disabledPeriodic()"); + // Register the opmode's additional periodic callbacks immediately on creation + m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); + m_callbacks.addAll(m_activeOpModeCallbacks); + + if (!enabled) { + // Call disabledPeriodic immediately for newly created OpMode when disabled + m_currentOpMode.disabledPeriodic(); + m_watchdog.addEpoch("opMode.disabledPeriodic()"); + calledOpModeDisabledPeriodicThisIteration = true; + } } + // Update m_lastModeId immediately to prevent the opmode change logic + // from destroying this OpMode later in the same loop iteration + m_lastModeId = modeId; } else { DriverStationErrors.reportError("No OpMode found for mode " + modeId, false); } } - m_lastModeId = modeId; // Handle enabled state changes boolean justCalledDisabledInit = false; @@ -696,29 +697,19 @@ private void loopFunc() { disabledExit(); m_watchdog.addEpoch("disabledExit()"); if (m_currentOpMode != null) { - // Register the opmode's periodic callbacks + // Register the main opmode periodic callback m_currentOpModePeriodic = m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); - m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); - m_callbacks.addAll(m_activeOpModeCallbacks); // Start the opmode + System.out.println("********** Starting OpMode " + m_currentOpModeName + " **********"); m_currentOpMode.start(); m_watchdog.addEpoch("opMode.start()"); } } else { // Transitioning to disabled if (m_currentOpMode != null) { - // Was enabled, now disabled - m_currentOpMode.end(); - m_watchdog.addEpoch("opMode.end()"); - - // Remove opmode callbacks - m_callbacks.remove(m_currentOpModePeriodic); - m_callbacks.removeAll(m_activeOpModeCallbacks); - m_activeOpModeCallbacks.clear(); - m_currentOpMode.close(); - m_currentOpMode = null; + endCurrentOpMode(); } disabledInit(); m_watchdog.addEpoch("disabledInit()"); @@ -733,17 +724,24 @@ private void loopFunc() { if (!justCalledDisabledInit) { disabledPeriodic(); m_watchdog.addEpoch("disabledPeriodic()"); + + // Handle opmode changes while disabled + if (modeId != m_lastModeId && m_currentOpMode != null) { + endCurrentOpMode(); + } } - // Call opmode disabledPeriodic if we have one - if (m_currentOpMode != null) { + // Call opmode disabledPeriodic if we have one and haven't called it already this iteration + if (m_currentOpMode != null && !calledOpModeDisabledPeriodicThisIteration) { m_currentOpMode.disabledPeriodic(); m_watchdog.addEpoch("opMode.disabledPeriodic()"); } } + m_lastModeId = modeId; + // Call nonePeriodic when no opmode is selected - if (RobotState.getOpModeId() == 0) { + if (modeId == 0) { nonePeriodic(); m_watchdog.addEpoch("nonePeriodic()"); } @@ -777,6 +775,28 @@ private void loopFunc() { } } + private void endCurrentOpMode() { + // if opmode was started + if (m_currentOpModePeriodic != null) { + System.out.println("********** Ending OpMode " + m_currentOpModeName + " **********"); + + m_currentOpMode.end(); + m_watchdog.addEpoch("opMode.end()"); + + // Remove opmode callbacks + m_callbacks.remove(m_currentOpModePeriodic); + m_callbacks.removeAll(m_activeOpModeCallbacks); + m_activeOpModeCallbacks.clear(); + m_currentOpModePeriodic = null; + } + + // regardless of whether opmode was started, close it and set to null + System.out.println("********** Closing OpMode " + m_currentOpModeName + " **********"); + m_currentOpMode.close(); + m_currentOpMode = null; + m_currentOpModeName = null; + } + /** Provide an alternate "main loop" via startCompetition(). */ @Override public final void startCompetition() { From 50736d1bcf3f044248091fb3767b142bc0d8d307 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 12 May 2026 22:28:40 -0400 Subject: [PATCH 04/10] Refactor OpModeLifecycleTest to improve readability and consistency in callback handling Signed-off-by: Zach Harel --- .../cpp/framework/OpModeLifecycleTest.cpp | 68 +++++++++------ .../wpilib/framework/OpModeLifecycleTest.java | 82 ++++++++++--------- 2 files changed, 86 insertions(+), 64 deletions(-) diff --git a/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp index b408c0d8d3e..f5084515559 100644 --- a/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp +++ b/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp @@ -3,10 +3,12 @@ // the WPILib BSD license file in the root directory of this project. #include +#include #include #include +#include "wpi/driverstation/RobotState.hpp" #include "wpi/framework/OpModeRobot.hpp" #include "wpi/hal/DriverStationTypes.h" #include "wpi/simulation/DriverStationSim.hpp" @@ -55,22 +57,23 @@ class OpModeLifecycleTest : public ::testing::Test { wpi::sim::DriverStationSim::ResetData(); wpi::sim::DriverStationSim::SetDsAttached(true); wpi::sim::DriverStationSim::SetEnabled(false); + wpi::RobotState::ClearOpModes(); } - void TearDown() override { - wpi::sim::ResumeTiming(); - } + void TearDown() override { wpi::sim::ResumeTiming(); } static int64_t MakeOpModeId(wpi::RobotMode mode, std::string_view name) { - return HAL_MakeOpModeId(static_cast(mode), std::hash{}(name)); + return HAL_MakeOpModeId(static_cast(mode), + std::hash{}(name)); } }; TEST_F(OpModeLifecycleTest, EnabledTransition) { Counts counts; LifecycleRobot robot; - robot.AddOpModeFactory([&] { return std::make_unique(counts); }, - wpi::RobotMode::TELEOPERATED, "TestOpMode"); + robot.AddOpModeFactory( + [&] { return std::make_unique(counts); }, + wpi::RobotMode::TELEOPERATED, "TestOpMode"); robot.PublishOpModes(); std::thread robotThread{[&] { robot.StartCompetition(); }}; @@ -82,16 +85,19 @@ TEST_F(OpModeLifecycleTest, EnabledTransition) { wpi::sim::StepTiming(20_ms); // Let the DS attached state propagate // 1. Selected, but disabled - wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "TestOpMode")); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TELEOPERATED); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "TestOpMode")); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(20_ms); EXPECT_EQ(counts.constructed.load(), 1u); - EXPECT_EQ(counts.disabledPeriodic.load(), 2u); + EXPECT_EQ(counts.disabledPeriodic.load(), 1u); // 2. Transition to enabled wpi::sim::DriverStationSim::SetEnabled(true); wpi::sim::DriverStationSim::NotifyNewData(); - wpi::sim::StepTiming(20_ms); + wpi::sim::StepTiming( + 40_ms); // Step twice like Java test to get periodic callback EXPECT_EQ(counts.start.load(), 1u); EXPECT_EQ(counts.periodic.load(), 1u); @@ -110,10 +116,12 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileEnabled) { Counts counts1; Counts counts2; LifecycleRobot robot; - robot.AddOpModeFactory([&] { return std::make_unique(counts1); }, - wpi::RobotMode::TELEOPERATED, "OpMode1"); - robot.AddOpModeFactory([&] { return std::make_unique(counts2); }, - wpi::RobotMode::TELEOPERATED, "OpMode2"); + robot.AddOpModeFactory( + [&] { return std::make_unique(counts1); }, + wpi::RobotMode::TELEOPERATED, "OpMode1"); + robot.AddOpModeFactory( + [&] { return std::make_unique(counts2); }, + wpi::RobotMode::TELEOPERATED, "OpMode2"); robot.PublishOpModes(); std::thread robotThread{[&] { robot.StartCompetition(); }}; @@ -124,25 +132,28 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileEnabled) { wpi::sim::DriverStationSim::NotifyNewData(); // 1. Select OpMode1 and enable - wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TELEOPERATED); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); wpi::sim::DriverStationSim::SetEnabled(true); wpi::sim::DriverStationSim::NotifyNewData(); - wpi::sim::StepTiming(20_ms); + wpi::sim::StepTiming(40_ms); // Need two iterations for periodic callback EXPECT_EQ(counts1.constructed.load(), 1u); EXPECT_EQ(counts1.start.load(), 1u); EXPECT_EQ(counts1.periodic.load(), 1u); // 2. Change to OpMode2 while enabled - wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); wpi::sim::DriverStationSim::NotifyNewData(); - wpi::sim::StepTiming(20_ms); + wpi::sim::StepTiming(40_ms); // OpMode1 should be ended and destructed EXPECT_EQ(counts1.end.load(), 1u); EXPECT_EQ(counts1.destructed.load(), 1u); // OpMode2 should be started EXPECT_EQ(counts2.constructed.load(), 1u); EXPECT_EQ(counts2.start.load(), 1u); - EXPECT_EQ(counts2.periodic.load(), 1u); + EXPECT_EQ(counts2.periodic.load(), 2u); robot.EndCompetition(); robotThread.join(); @@ -152,10 +163,12 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileDisabled) { Counts counts1; Counts counts2; LifecycleRobot robot; - robot.AddOpModeFactory([&] { return std::make_unique(counts1); }, - wpi::RobotMode::TELEOPERATED, "OpMode1"); - robot.AddOpModeFactory([&] { return std::make_unique(counts2); }, - wpi::RobotMode::TELEOPERATED, "OpMode2"); + robot.AddOpModeFactory( + [&] { return std::make_unique(counts1); }, + wpi::RobotMode::TELEOPERATED, "OpMode1"); + robot.AddOpModeFactory( + [&] { return std::make_unique(counts2); }, + wpi::RobotMode::TELEOPERATED, "OpMode2"); robot.PublishOpModes(); std::thread robotThread{[&] { robot.StartCompetition(); }}; @@ -165,14 +178,17 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileDisabled) { wpi::sim::DriverStationSim::SetDsAttached(true); // 1. Select OpMode1 while disabled - wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TELEOPERATED); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode1")); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(20_ms); EXPECT_EQ(counts1.constructed.load(), 1u); - EXPECT_EQ(counts1.disabledPeriodic.load(), 2u); + EXPECT_EQ(counts1.disabledPeriodic.load(), 1u); // 2. Change to OpMode2 while disabled - wpi::sim::DriverStationSim::SetOpMode(MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(20_ms); // OpMode1 should be destructed, but NOT ended @@ -180,7 +196,7 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileDisabled) { EXPECT_EQ(counts1.end.load(), 0u); // OpMode2 should be selected EXPECT_EQ(counts2.constructed.load(), 1u); - EXPECT_EQ(counts2.disabledPeriodic.load(), 2u); + EXPECT_EQ(counts2.disabledPeriodic.load(), 1u); robot.EndCompetition(); robotThread.join(); diff --git a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java index 344e491a01f..47c57cc12f9 100644 --- a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java +++ b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java @@ -26,53 +26,53 @@ private static long makeOpModeId(RobotMode mode, String name) { return OpModeOption.makeId(mode, name.hashCode()); } - static class LifecycleOpMode implements OpMode { - public final AtomicInteger m_constructedCount; - public final AtomicInteger m_disabledPeriodicCount; - public final AtomicInteger m_startCount; - public final AtomicInteger m_periodicCount; - public final AtomicInteger m_endCount; - public final AtomicInteger m_closeCount; - - LifecycleOpMode( + static class MockOpMode implements OpMode { + public final AtomicInteger constructedCount; + public final AtomicInteger disabledPeriodicCount; + public final AtomicInteger startCount; + public final AtomicInteger periodicCount; + public final AtomicInteger endCount; + public final AtomicInteger closeCount; + + MockOpMode( AtomicInteger constructedCount, AtomicInteger disabledPeriodicCount, AtomicInteger startCount, AtomicInteger periodicCount, AtomicInteger endCount, AtomicInteger closeCount) { - m_constructedCount = constructedCount; - m_disabledPeriodicCount = disabledPeriodicCount; - m_startCount = startCount; - m_periodicCount = periodicCount; - m_endCount = endCount; - m_closeCount = closeCount; - m_constructedCount.incrementAndGet(); + this.constructedCount = constructedCount; + this.disabledPeriodicCount = disabledPeriodicCount; + this.startCount = startCount; + this.periodicCount = periodicCount; + this.endCount = endCount; + this.closeCount = closeCount; + this.constructedCount.incrementAndGet(); } @Override public void disabledPeriodic() { - m_disabledPeriodicCount.incrementAndGet(); + disabledPeriodicCount.incrementAndGet(); } @Override public void start() { - m_startCount.incrementAndGet(); + startCount.incrementAndGet(); } @Override public void periodic() { - m_periodicCount.incrementAndGet(); + periodicCount.incrementAndGet(); } @Override public void end() { - m_endCount.incrementAndGet(); + endCount.incrementAndGet(); } @Override public void close() { - m_closeCount.incrementAndGet(); + closeCount.incrementAndGet(); } } @@ -109,7 +109,7 @@ void testLifecycleEnabledTransition() throws InterruptedException { LifecycleRobot robot = new LifecycleRobot(); robot.addOpModeFactory( () -> - new LifecycleOpMode( + new MockOpMode( constructedCount, disabledPeriodicCount, startCount, @@ -119,27 +119,26 @@ void testLifecycleEnabledTransition() throws InterruptedException { RobotMode.TELEOPERATED, "TestOpMode"); robot.publishOpModes(); - var options = DriverStationSim.getOpModeOptions(); - var id = makeOpModeId(RobotMode.TELEOPERATED, "TestOpMode"); - assertEquals(1, options.length); - assertEquals(id, options[0].id); Thread robotThread = new Thread(robot::startCompetition); robotThread.start(); SimHooks.waitForProgramStart(); // 1. Selected, but disabled - DriverStationSim.setOpMode(id); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "TestOpMode")); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); assertEquals(1, constructedCount.get()); - assertEquals(2, disabledPeriodicCount.get()); + assertEquals(1, disabledPeriodicCount.get()); // 2. Transition to enabled DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(kPeriod); + SimHooks.stepTiming(2 * kPeriod); + // Starts on first loop assertEquals(1, startCount.get()); + // Periodic is called on second loop assertEquals(1, periodicCount.get()); // 3. Transition to disabled @@ -174,7 +173,7 @@ void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { LifecycleRobot robot = new LifecycleRobot(); robot.addOpModeFactory( () -> - new LifecycleOpMode( + new MockOpMode( constructedCount1, disabledPeriodicCount1, startCount1, @@ -185,7 +184,7 @@ void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { "OpMode1"); robot.addOpModeFactory( () -> - new LifecycleOpMode( + new MockOpMode( constructedCount2, disabledPeriodicCount2, startCount2, @@ -201,22 +200,28 @@ void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { SimHooks.waitForProgramStart(); // 1. Select OpMode1 and enable + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode1")); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(kPeriod); + SimHooks.stepTiming(2 * kPeriod); assertEquals(1, constructedCount1.get()); assertEquals(1, startCount1.get()); assertEquals(1, periodicCount1.get()); // 2. Change to OpMode2 while enabled DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); + DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); // OpMode1 should be ended and closed assertEquals(1, endCount1.get()); assertEquals(1, closeCount1.get()); - // OpMode2 should be started + + // DS transitions to disabled on opmode switch, so enable again to start OpMode2 + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(2 * kPeriod); assertEquals(1, constructedCount2.get()); assertEquals(1, startCount2.get()); assertEquals(1, periodicCount2.get()); @@ -245,7 +250,7 @@ void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { LifecycleRobot robot = new LifecycleRobot(); robot.addOpModeFactory( () -> - new LifecycleOpMode( + new MockOpMode( constructedCount1, disabledPeriodicCount1, startCount1, @@ -256,7 +261,7 @@ void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { "OpMode1"); robot.addOpModeFactory( () -> - new LifecycleOpMode( + new MockOpMode( constructedCount2, disabledPeriodicCount2, startCount2, @@ -272,22 +277,23 @@ void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { SimHooks.waitForProgramStart(); // 1. Select OpMode1 while disabled + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode1")); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(kPeriod); + SimHooks.stepTiming(2 * kPeriod); assertEquals(1, constructedCount1.get()); assertEquals(2, disabledPeriodicCount1.get()); // 2. Change to OpMode2 while disabled DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(kPeriod); + SimHooks.stepTiming(2 * kPeriod); // OpMode1 should be closed, but NOT ended (since it never started) assertEquals(1, closeCount1.get()); assertEquals(0, endCount1.get()); // OpMode2 should be selected assertEquals(1, constructedCount2.get()); - assertEquals(2, disabledPeriodicCount2.get()); + assertEquals(1, disabledPeriodicCount2.get()); robot.endCompetition(); robotThread.join(); From 19acb8d867058cdfadf029521553e067417ebf92 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 12 May 2026 22:28:59 -0400 Subject: [PATCH 05/10] Enhance OpMode documentation to clarify registration timing and execution conditions for callbacks Signed-off-by: Zach Harel --- design-docs/opmodes.md | 5 +++-- wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp | 6 ++++++ wpilibj/src/main/java/org/wpilib/opmode/OpMode.java | 5 +++++ 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/design-docs/opmodes.md b/design-docs/opmodes.md index 7165e6a74ed..558a92fd01b 100644 --- a/design-docs/opmodes.md +++ b/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 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) {...} } diff --git a/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp index 48f1d8716e3..8b45dbfed58 100644 --- a/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp +++ b/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. * + *

Registration timing: 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. diff --git a/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java index 00427992f34..f0a3a75bcf2 100644 --- a/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java +++ b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java @@ -75,6 +75,11 @@ default void close() {} * intervals. The callbacks are executed by the robot framework at their scheduled times, in * addition to the primary {@link #periodic()} callback. * + *

Registration timing: 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 set of custom callbacks to execute, or an empty set if no custom callbacks are * needed. The default implementation returns an empty set. */ From c67127a70265d815fccc04ab6693131262f4e4d5 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 12 May 2026 22:40:38 -0400 Subject: [PATCH 06/10] Reset simulation data and clear OpModes in OpModeRobotTest setup Signed-off-by: Zach Harel --- .../test/native/cpp/{framework => }/OpModeLifecycleTest.cpp | 0 wpilibc/src/test/native/cpp/OpModeRobotTest.cpp | 3 +++ 2 files changed, 3 insertions(+) rename wpilibc/src/test/native/cpp/{framework => }/OpModeLifecycleTest.cpp (100%) diff --git a/wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/framework/OpModeLifecycleTest.cpp rename to wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp diff --git a/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp index cfe0444f402..d82c8944719 100644 --- a/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp @@ -8,6 +8,7 @@ #include +#include "wpi/driverstation/RobotState.hpp" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/SimHooks.hpp" #include "wpi/util/Color.hpp" @@ -21,6 +22,8 @@ class OpModeRobotTest : public ::testing::Test { void SetUp() override { wpi::sim::PauseTiming(); wpi::sim::SetProgramStarted(false); + wpi::sim::DriverStationSim::ResetData(); + wpi::RobotState::ClearOpModes(); } void TearDown() override { From 5dbfb0b983bba88ed0ecaeb8a385d4210f874520 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 19 May 2026 10:25:47 -0400 Subject: [PATCH 07/10] Fix OpMode periodic callback handling to prevent circular references Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 18 +++++++++++++----- .../test/native/cpp/OpModeLifecycleTest.cpp | 2 +- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index f312ece44ce..87159ce3239 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -106,8 +106,12 @@ void OpModeRobotBase::LoopFunc() { fmt::print("********** Starting OpMode **********\n"); // Register the main opmode periodic callback m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ - [op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime, - m_period}; + [op = std::weak_ptr{m_currentOpMode}] { + if (auto shared_op = op.lock()) { + shared_op->Periodic(); + } + }, + m_startTime, m_period}; m_callbacks.Add(*m_opmodePeriodic); m_currentOpMode->Start(); @@ -133,8 +137,12 @@ void OpModeRobotBase::LoopFunc() { // Only start if not already started // Register the main opmode periodic callback m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ - [op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime, - m_period}; + [op = std::weak_ptr{m_currentOpMode}] { + if (auto shared_op = op.lock()) { + shared_op->Periodic(); + } + }, + m_startTime, m_period}; m_callbacks.Add(*m_opmodePeriodic); // Start the opmode @@ -291,7 +299,7 @@ void OpModeRobotBase::EndCurrentOpMode() { m_currentOpMode->End(); m_watchdog.AddEpoch("OpMode::End()"); - // Remove opmode callbacks + // Remove opmode callbacks first to break circular references m_callbacks.Remove(*m_opmodePeriodic); m_opmodePeriodic.reset(); for (auto& cb : m_activeOpModeCallbacks) { diff --git a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp index f5084515559..9c4bff92525 100644 --- a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp +++ b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp @@ -153,7 +153,7 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileEnabled) { // OpMode2 should be started EXPECT_EQ(counts2.constructed.load(), 1u); EXPECT_EQ(counts2.start.load(), 1u); - EXPECT_EQ(counts2.periodic.load(), 2u); + EXPECT_EQ(counts2.periodic.load(), 1u); robot.EndCompetition(); robotThread.join(); From 73ff0cc07e305f28a1a0101802380fc9ebfa23fb Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Sun, 24 May 2026 15:20:08 -0400 Subject: [PATCH 08/10] Fix OpMode periodic callback handling to prevent circular references Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 43 +++++++++---------- .../org/wpilib/framework/OpModeRobot.java | 10 ++--- 2 files changed, 24 insertions(+), 29 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index 87159ce3239..cd9c9149310 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -94,29 +94,26 @@ void OpModeRobotBase::LoopFunc() { m_callbacks.Add(cb); } - if (!enabled) { - // Call DisabledPeriodic immediately for newly created OpMode when - // disabled - m_currentOpMode->DisabledPeriodic(); - m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); - calledOpModeDisabledPeriodicThisIteration = true; - } else { - // If robot is enabled, start the OpMode immediately - if (!m_opmodePeriodic) { - fmt::print("********** Starting OpMode **********\n"); - // Register the main opmode periodic callback - m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ - [op = std::weak_ptr{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()"); - } + // Call DisabledPeriodic immediately for newly created OpMode when + // disabled + m_currentOpMode->DisabledPeriodic(); + m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); + calledOpModeDisabledPeriodicThisIteration = true; + + if (enabled && !m_opmodePeriodic) { + fmt::print("********** Starting OpMode **********\n"); + // Register the main opmode periodic callback + m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ + [op = std::weak_ptr{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()"); } } // Update m_lastModeId immediately to prevent race conditions diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index ba8c1a9711f..eb675b95efb 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -674,12 +674,10 @@ private void loopFunc() { m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks()); m_callbacks.addAll(m_activeOpModeCallbacks); - if (!enabled) { - // Call disabledPeriodic immediately for newly created OpMode when disabled - m_currentOpMode.disabledPeriodic(); - m_watchdog.addEpoch("opMode.disabledPeriodic()"); - calledOpModeDisabledPeriodicThisIteration = true; - } + // Call disabledPeriodic immediately for newly created OpMode when disabled + m_currentOpMode.disabledPeriodic(); + m_watchdog.addEpoch("opMode.disabledPeriodic()"); + calledOpModeDisabledPeriodicThisIteration = true; } // Update m_lastModeId immediately to prevent the opmode change logic // from destroying this OpMode later in the same loop iteration From ec78578c9ca08ebcce064a58238c997f1e5f731a Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 9 Jun 2026 16:47:52 -0400 Subject: [PATCH 09/10] refactor opmode lifecycle management so c++ and java match again Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 96 ++++++++----------- .../include/wpi/framework/OpModeRobot.hpp | 8 ++ .../src/main/python/semiwrap/OpModeRobot.yml | 3 + .../test/native/cpp/OpModeLifecycleTest.cpp | 16 +++- .../org/wpilib/framework/OpModeRobot.java | 50 ++++++---- .../wpilib/framework/OpModeLifecycleTest.java | 14 ++- 6 files changed, 103 insertions(+), 84 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index cd9c9149310..f397d8a629b 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -78,13 +78,12 @@ void OpModeRobotBase::LoopFunc() { // Set up new opmode if (modeId != 0 && !m_currentOpMode) { - fmt::print( - "DEBUG: Looking for OpMode with ID {} in registry with {} entries\n", - modeId, m_opModes.size()); auto data = m_opModes.lookup(modeId); if (data.factory) { // Instantiate the new opmode - fmt::print("********** Creating OpMode {} **********\n", data.name); + 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 @@ -99,25 +98,7 @@ void OpModeRobotBase::LoopFunc() { m_currentOpMode->DisabledPeriodic(); m_watchdog.AddEpoch("OpMode::DisabledPeriodic()"); calledOpModeDisabledPeriodicThisIteration = true; - - if (enabled && !m_opmodePeriodic) { - fmt::print("********** Starting OpMode **********\n"); - // Register the main opmode periodic callback - m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ - [op = std::weak_ptr{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()"); - } } - // Update m_lastModeId immediately to prevent race conditions - m_lastModeId = modeId; } else { WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); } @@ -130,26 +111,11 @@ void OpModeRobotBase::LoopFunc() { // Transitioning to enabled DisabledExit(); m_watchdog.AddEpoch("DisabledExit()"); - if (m_currentOpMode && !m_opmodePeriodic) { - // Only start if not already started - // Register the main opmode periodic callback - m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{ - [op = std::weak_ptr{m_currentOpMode}] { - if (auto shared_op = op.lock()) { - shared_op->Periodic(); - } - }, - m_startTime, m_period}; - m_callbacks.Add(*m_opmodePeriodic); - - // Start the opmode - fmt::print("********** Starting OpMode **********\n"); - m_currentOpMode->Start(); - m_watchdog.AddEpoch("OpMode::Start()"); - } } else { - // Transitioning to disabled - if (m_currentOpMode) { + // 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(); @@ -159,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 @@ -175,9 +148,10 @@ void OpModeRobotBase::LoopFunc() { } } + m_lastModeId = modeId; + // Call NonePeriodic when no opmode is selected if (modeId == 0) { - m_lastModeId = modeId; NonePeriodic(); m_watchdog.AddEpoch("NonePeriodic()"); } @@ -242,13 +216,7 @@ void OpModeRobotBase::AddOpModeFactory( int64_t id = RobotState::AddOpMode(mode, name, group, description, textColor, backgroundColor); if (id != 0) { - fmt::print("DEBUG: Registering OpMode '{}' with ID {}\n", name, id); m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; - } else { - fmt::print( - "DEBUG: Failed to register OpMode '{}' - RobotState::AddOpMode " - "returned 0\n", - name); } } @@ -258,13 +226,7 @@ void OpModeRobotBase::AddOpModeFactory(OpModeFactory factory, RobotMode mode, std::string_view description) { int64_t id = RobotState::AddOpMode(mode, name, group, description); if (id != 0) { - fmt::print("DEBUG: Registering OpMode '{}' with ID {}\n", name, id); m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; - } else { - fmt::print( - "DEBUG: Failed to register OpMode '{}' - RobotState::AddOpMode " - "returned 0\n", - name); } } @@ -284,6 +246,28 @@ void OpModeRobotBase::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{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; @@ -291,7 +275,7 @@ void OpModeRobotBase::EndCurrentOpMode() { // If opmode was started (enabled) if (m_opmodePeriodic) { - fmt::print("********** Ending OpMode **********\n"); + fmt::print("********** Ending OpMode {} **********\n", m_currentOpModeName); m_currentOpMode->End(); m_watchdog.AddEpoch("OpMode::End()"); @@ -306,6 +290,6 @@ void OpModeRobotBase::EndCurrentOpMode() { } // Regardless of whether opmode was started, destroy it - fmt::print("********** Closing OpMode **********\n"); + fmt::print("********** Closing OpMode {} **********\n", m_currentOpModeName); m_currentOpMode.reset(); } diff --git a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp index 172e676c6fb..c9ac627fa61 100644 --- a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp @@ -220,6 +220,13 @@ 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. */ @@ -246,6 +253,7 @@ class OpModeRobotBase : public RobotBase { bool m_calledDriverStationConnected = false; bool m_lastEnabledState = false; std::shared_ptr m_currentOpMode; + std::string m_currentOpModeName; std::vector m_activeOpModeCallbacks; std::optional diff --git a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml index c6d316a9249..fa4a6695f8c 100644 --- a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml +++ b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml @@ -29,7 +29,10 @@ classes: AddPeriodic: GetLoopStartTime: LoopFunc: + StartCurrentOpMode: + ignore: true EndCurrentOpMode: + ignore: true attributes: DEFAULT_PERIOD: wpi::OpModeRobot: diff --git a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp index 9c4bff92525..44ef93b9f97 100644 --- a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp +++ b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp @@ -142,15 +142,25 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileEnabled) { EXPECT_EQ(counts1.start.load(), 1u); EXPECT_EQ(counts1.periodic.load(), 1u); - // 2. Change to OpMode2 while enabled + // 2. Switch to OpMode2 while enabled. Selecting a different opmode while + // enabled disables the robot first, so the DS sends disabled + new opmode. wpi::sim::DriverStationSim::SetOpMode( MakeOpModeId(wpi::RobotMode::TELEOPERATED, "OpMode2")); + wpi::sim::DriverStationSim::SetEnabled(false); wpi::sim::DriverStationSim::NotifyNewData(); - wpi::sim::StepTiming(40_ms); + wpi::sim::StepTiming(20_ms); // OpMode1 should be ended and destructed EXPECT_EQ(counts1.end.load(), 1u); EXPECT_EQ(counts1.destructed.load(), 1u); - // OpMode2 should be started + // OpMode2 should be constructed exactly once and persist while disabled + EXPECT_EQ(counts2.constructed.load(), 1u); + EXPECT_EQ(counts2.start.load(), 0u); + + // 3. Re-enable. The same OpMode2 instance is started; it is not + // reconstructed. + wpi::sim::DriverStationSim::SetEnabled(true); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(40_ms); EXPECT_EQ(counts2.constructed.load(), 1u); EXPECT_EQ(counts2.start.load(), 1u); EXPECT_EQ(counts2.periodic.load(), 1u); diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index eb675b95efb..bee48a7b672 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -662,6 +662,11 @@ private void loopFunc() { m_watchdog.addEpoch("driverStationConnected()"); } + // Handle opmode changes: tear down the old opmode if the selection changed + if (modeId != m_lastModeId && m_currentOpMode != null) { + endCurrentOpMode(); + } + // Set up new opmode if (modeId != 0 && m_currentOpMode == null) { OpModeFactory factory = m_opModes.get(modeId); @@ -679,9 +684,6 @@ private void loopFunc() { m_watchdog.addEpoch("opMode.disabledPeriodic()"); calledOpModeDisabledPeriodicThisIteration = true; } - // Update m_lastModeId immediately to prevent the opmode change logic - // from destroying this OpMode later in the same loop iteration - m_lastModeId = modeId; } else { DriverStationErrors.reportError("No OpMode found for mode " + modeId, false); } @@ -694,19 +696,11 @@ private void loopFunc() { // Transitioning to enabled disabledExit(); m_watchdog.addEpoch("disabledExit()"); - if (m_currentOpMode != null) { - // Register the main opmode periodic callback - m_currentOpModePeriodic = - m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); - - // Start the opmode - System.out.println("********** Starting OpMode " + m_currentOpModeName + " **********"); - m_currentOpMode.start(); - m_watchdog.addEpoch("opMode.start()"); - } } else { - // Transitioning to disabled - if (m_currentOpMode != null) { + // 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 != null && m_currentOpModePeriodic != null) { endCurrentOpMode(); } disabledInit(); @@ -716,17 +710,19 @@ private void 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 != null && m_currentOpModePeriodic == null) { + startCurrentOpMode(); + } + // Call periodic functions based on current state if (!enabled) { // Only call disabledPeriodic if we didn't just call disabledInit if (!justCalledDisabledInit) { disabledPeriodic(); m_watchdog.addEpoch("disabledPeriodic()"); - - // Handle opmode changes while disabled - if (modeId != m_lastModeId && m_currentOpMode != null) { - endCurrentOpMode(); - } } // Call opmode disabledPeriodic if we have one and haven't called it already this iteration @@ -773,6 +769,20 @@ private void loopFunc() { } } + private void startCurrentOpMode() { + if (m_currentOpMode == null || m_currentOpModePeriodic != null) { + return; + } + + System.out.println("********** Starting OpMode " + m_currentOpModeName + " **********"); + + // Register the main opmode periodic callback + m_currentOpModePeriodic = m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period); + + m_currentOpMode.start(); + m_watchdog.addEpoch("opMode.start()"); + } + private void endCurrentOpMode() { // if opmode was started if (m_currentOpModePeriodic != null) { diff --git a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java index 47c57cc12f9..4aeb82f7e8c 100644 --- a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java +++ b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java @@ -209,7 +209,8 @@ void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { assertEquals(1, startCount1.get()); assertEquals(1, periodicCount1.get()); - // 2. Change to OpMode2 while enabled + // 2. Switch to OpMode2 while enabled. Selecting a different opmode while + // enabled disables the robot first, so the DS sends disabled + new opmode. DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); @@ -217,8 +218,11 @@ void testLifecycleOpModeChangeWhileEnabled() throws InterruptedException { // OpMode1 should be ended and closed assertEquals(1, endCount1.get()); assertEquals(1, closeCount1.get()); + // OpMode2 should be constructed exactly once and persist while disabled + assertEquals(1, constructedCount2.get()); + assertEquals(0, startCount2.get()); - // DS transitions to disabled on opmode switch, so enable again to start OpMode2 + // 3. Re-enable. The same OpMode2 instance is started; it is not reconstructed. DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); SimHooks.stepTiming(2 * kPeriod); @@ -280,14 +284,14 @@ void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode1")); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(2 * kPeriod); + SimHooks.stepTiming(kPeriod); assertEquals(1, constructedCount1.get()); - assertEquals(2, disabledPeriodicCount1.get()); + assertEquals(1, disabledPeriodicCount1.get()); // 2. Change to OpMode2 while disabled DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "OpMode2")); DriverStationSim.notifyNewData(); - SimHooks.stepTiming(2 * kPeriod); + SimHooks.stepTiming(kPeriod); // OpMode1 should be closed, but NOT ended (since it never started) assertEquals(1, closeCount1.get()); assertEquals(0, endCount1.get()); From f892d7043b51f518b26945c64e1aa832e841cdc8 Mon Sep 17 00:00:00 2001 From: Zach Harel Date: Tue, 9 Jun 2026 18:21:49 -0400 Subject: [PATCH 10/10] Add CallbackOpMode to verify immediate execution of callbacks while disabled Signed-off-by: Zach Harel --- .../main/native/cpp/framework/OpModeRobot.cpp | 15 +++-- .../cpp/internal/PeriodicPriorityQueue.cpp | 11 +++- .../wpi/internal/PeriodicPriorityQueue.hpp | 11 +++- .../python/semiwrap/PeriodicPriorityQueue.yml | 1 + .../test/native/cpp/OpModeLifecycleTest.cpp | 56 +++++++++++++++++++ .../org/wpilib/framework/OpModeRobot.java | 10 ++-- .../internal/PeriodicPriorityQueue.java | 21 +++++-- .../wpilib/framework/OpModeLifecycleTest.java | 54 ++++++++++++++++++ 8 files changed, 160 insertions(+), 19 deletions(-) diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp index f397d8a629b..53f2a33b7b2 100644 --- a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -273,22 +273,25 @@ void OpModeRobotBase::EndCurrentOpMode() { return; } - // If opmode was started (enabled) + // 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()"); - // Remove opmode callbacks first to break circular references m_callbacks.Remove(*m_opmodePeriodic); m_opmodePeriodic.reset(); - for (auto& cb : m_activeOpModeCallbacks) { - m_callbacks.Remove(cb); - } - m_activeOpModeCallbacks.clear(); } + // 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(); diff --git a/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp b/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp index 55900668cff..6d3b4561701 100644 --- a/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp +++ b/wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp @@ -4,6 +4,8 @@ #include "wpi/internal/PeriodicPriorityQueue.hpp" +#include +#include #include #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 gNextCallbackId{1}; +} // namespace + PeriodicPriorityQueue::Callback::Callback(std::function func, std::chrono::microseconds startTime, std::chrono::microseconds period, @@ -23,7 +31,8 @@ PeriodicPriorityQueue::Callback::Callback(std::function 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 func, std::chrono::microseconds startTime, diff --git a/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp b/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp index 3f6c6e6eea8..c8112ba3a37 100644 --- a/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp +++ b/wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include @@ -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; } }; /** diff --git a/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml b/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml index 369920f5a45..9fb74a91706 100644 --- a/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml +++ b/wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml @@ -22,6 +22,7 @@ classes: func: period: expirationTime: + id: methods: Callback: overloads: diff --git a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp index 44ef93b9f97..e53a1e9b7e3 100644 --- a/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp +++ b/wpilibc/src/test/native/cpp/OpModeLifecycleTest.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -49,6 +50,26 @@ class LifecycleRobot : public wpi::OpModeRobot { LifecycleRobot() = default; }; +// OpMode whose GetCallbacks() returns a single callback with no enabled check, +// used to verify that getCallbacks() callbacks are registered immediately and +// run even while disabled. +class CallbackOpMode : public wpi::OpMode { + public: + explicit CallbackOpMode(std::atomic& callbackCount) + : m_callbackCount(callbackCount) {} + + std::vector GetCallbacks() + override { + std::vector callbacks; + callbacks.emplace_back([&count = m_callbackCount] { count++; }, + std::chrono::microseconds{0}, 20_ms); + return callbacks; + } + + private: + std::atomic& m_callbackCount; +}; + class OpModeLifecycleTest : public ::testing::Test { protected: void SetUp() override { @@ -212,4 +233,39 @@ TEST_F(OpModeLifecycleTest, OpModeChangeWhileDisabled) { robotThread.join(); } +TEST_F(OpModeLifecycleTest, GetCallbacksRunImmediatelyWhileDisabled) { + std::atomic callbackCount{0}; + LifecycleRobot robot; + robot.AddOpModeFactory( + [&] { return std::make_unique(callbackCount); }, + wpi::RobotMode::TELEOPERATED, "CallbackOpMode"); + robot.PublishOpModes(); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + wpi::sim::DriverStationSim::SetDsAttached(true); + + // Select the opmode while disabled. getCallbacks() callbacks are registered + // immediately on construction and run even while the robot is disabled. + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TELEOPERATED); + wpi::sim::DriverStationSim::SetOpMode( + MakeOpModeId(wpi::RobotMode::TELEOPERATED, "CallbackOpMode")); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(100_ms); + EXPECT_GE(callbackCount.load(), 1u); + + // Deselecting the opmode tears it down and removes its callbacks, so the + // callback must stop running. + wpi::sim::DriverStationSim::SetOpMode(0); + wpi::sim::DriverStationSim::NotifyNewData(); + wpi::sim::StepTiming(100_ms); // let teardown settle + uint32_t countAfterTeardown = callbackCount.load(); + wpi::sim::StepTiming(100_ms); + EXPECT_EQ(callbackCount.load(), countAfterTeardown); + + robot.EndCompetition(); + robotThread.join(); +} + } // namespace diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java index bee48a7b672..190bf8ef54d 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -784,20 +784,22 @@ private void startCurrentOpMode() { } private void endCurrentOpMode() { - // if opmode was started + // If the opmode was started, end it and remove its main periodic callback. if (m_currentOpModePeriodic != null) { System.out.println("********** Ending OpMode " + m_currentOpModeName + " **********"); m_currentOpMode.end(); m_watchdog.addEpoch("opMode.end()"); - // Remove opmode callbacks m_callbacks.remove(m_currentOpModePeriodic); - m_callbacks.removeAll(m_activeOpModeCallbacks); - m_activeOpModeCallbacks.clear(); m_currentOpModePeriodic = null; } + // The additional getCallbacks() callbacks are registered immediately on construction (even + // while disabled), so always remove them regardless of whether the opmode was started. + m_callbacks.removeAll(m_activeOpModeCallbacks); + m_activeOpModeCallbacks.clear(); + // regardless of whether opmode was started, close it and set to null System.out.println("********** Closing OpMode " + m_currentOpModeName + " **********"); m_currentOpMode.close(); diff --git a/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java b/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java index 389117d3e9e..66771a5d0ba 100644 --- a/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java +++ b/wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java @@ -6,6 +6,7 @@ import java.util.Collection; import java.util.PriorityQueue; +import java.util.concurrent.atomic.AtomicLong; import org.wpilib.hardware.hal.NotifierJNI; import org.wpilib.system.RobotController; import org.wpilib.util.WPIUtilJNI; @@ -220,6 +221,9 @@ public long getLoopStartTime() { * when execution is delayed. */ public static class Callback implements Comparable { + /** Source of unique callback ids. */ + private static final AtomicLong s_nextId = new AtomicLong(1); + /** The function to execute when the callback fires. */ public final Runnable func; @@ -229,6 +233,12 @@ public static class Callback implements Comparable { /** The next scheduled execution time in monotonic timestamp microseconds. */ public long expirationTime; + /** + * The unique id for this callback to allow callbacks to be tracked and removed from the queue + * throughout robot operation. + */ + public final long id; + /** * Construct a callback container. * @@ -245,6 +255,7 @@ public Callback(Runnable func, long startTime, long period, long offset) { + offset + (1 + (RobotController.getMonotonicTime() - startTime - offset) / this.period) * this.period; + this.id = s_nextId.getAndIncrement(); } /** @@ -271,24 +282,24 @@ public Callback(Runnable func, long timestamp, double periodSeconds) { } /** - * Compares callbacks based on expiration time for equality. + * Compares callbacks based on their stable identity. * * @param rhs The object to compare against. - * @return true if rhs is a Callback with the same expiration time. + * @return true if rhs is the same Callback (same id). */ @Override public boolean equals(Object rhs) { - return rhs instanceof Callback callback && expirationTime == callback.expirationTime; + return rhs instanceof Callback callback && id == callback.id; } /** - * Returns a hash code based on the expiration time. + * Returns a hash code based on the stable identity. * * @return hash code for this callback. */ @Override public int hashCode() { - return Long.hashCode(expirationTime); + return Long.hashCode(id); } /** diff --git a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java index 4aeb82f7e8c..8499f427d45 100644 --- a/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java +++ b/wpilibj/src/test/java/org/wpilib/framework/OpModeLifecycleTest.java @@ -5,7 +5,9 @@ package org.wpilib.framework; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; +import java.util.Set; import java.util.concurrent.atomic.AtomicInteger; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; @@ -14,6 +16,7 @@ import org.wpilib.driverstation.internal.DriverStationBackend; import org.wpilib.hardware.hal.OpModeOption; import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.internal.PeriodicPriorityQueue; import org.wpilib.opmode.OpMode; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.SimHooks; @@ -82,6 +85,23 @@ static class LifecycleRobot extends OpModeRobot { } } + // OpMode whose getCallbacks() returns a single callback with no enabled check, + // used to verify that getCallbacks() callbacks are registered immediately and + // run even while disabled. + static class CallbackOpMode implements OpMode { + private final AtomicInteger m_callbackCount; + + CallbackOpMode(AtomicInteger callbackCount) { + m_callbackCount = callbackCount; + } + + @Override + public Set getCallbacks() { + return Set.of( + new PeriodicPriorityQueue.Callback(m_callbackCount::incrementAndGet, 0, kPeriod)); + } + } + @BeforeEach void setUp() { SimHooks.pauseTiming(); @@ -303,4 +323,38 @@ void testLifecycleOpModeChangeWhileDisabled() throws InterruptedException { robotThread.join(); robot.close(); } + + @Test + void testGetCallbacksRunImmediatelyWhileDisabled() throws InterruptedException { + AtomicInteger callbackCount = new AtomicInteger(0); + LifecycleRobot robot = new LifecycleRobot(); + robot.addOpModeFactory( + () -> new CallbackOpMode(callbackCount), RobotMode.TELEOPERATED, "CallbackOpMode"); + robot.publishOpModes(); + + Thread robotThread = new Thread(robot::startCompetition); + robotThread.start(); + SimHooks.waitForProgramStart(); + + // Select the opmode while disabled. getCallbacks() callbacks are registered + // immediately on construction and run even while the robot is disabled. + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); + DriverStationSim.setOpMode(makeOpModeId(RobotMode.TELEOPERATED, "CallbackOpMode")); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(5 * kPeriod); + assertTrue(callbackCount.get() >= 1); + + // Deselecting the opmode tears it down and removes its callbacks, so the + // callback must stop running. + DriverStationSim.setOpMode(0); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(5 * kPeriod); // let teardown settle + int countAfterTeardown = callbackCount.get(); + SimHooks.stepTiming(5 * kPeriod); + assertEquals(countAfterTeardown, callbackCount.get()); + + robot.endCompetition(); + robotThread.join(); + robot.close(); + } }