From 7508aada93aa04429b0600cc4e89ce6cd330244b Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 5 Nov 2019 21:33:09 -0800 Subject: [PATCH] Add ability to end startCompetition() main loop (#2032) This is useful for both cleanly exiting from simulation and for unit testing at a framework level. This change required removing move constructor/assignment from IterativeRobot. --- wpilibc/src/main/native/cpp/DriverStation.cpp | 15 ++--- .../src/main/native/cpp/IterativeRobot.cpp | 6 ++ wpilibc/src/main/native/cpp/TimedRobot.cpp | 5 ++ .../main/native/include/frc/DriverStation.h | 5 ++ .../main/native/include/frc/IterativeRobot.h | 15 +++-- .../src/main/native/include/frc/RobotBase.h | 55 +++++++++++++++--- .../src/main/native/include/frc/TimedRobot.h | 5 ++ .../templates/robotbaseskeleton/cpp/Robot.cpp | 4 +- .../robotbaseskeleton/include/Robot.h | 6 ++ .../edu/wpi/first/wpilibj/DriverStation.java | 16 ++++-- .../edu/wpi/first/wpilibj/IterativeRobot.java | 13 +++++ .../java/edu/wpi/first/wpilibj/RobotBase.java | 56 ++++++++++++++++--- .../edu/wpi/first/wpilibj/TimedRobot.java | 10 +++- .../templates/robotbaseskeleton/Robot.java | 9 ++- 14 files changed, 185 insertions(+), 35 deletions(-) diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index d268de7ad8..4e2739ff2e 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -473,6 +473,13 @@ double DriverStation::GetBatteryVoltage() const { return voltage; } +void DriverStation::WakeupWaitForData() { + std::scoped_lock waitLock(m_waitForDataMutex); + // Nofify all threads + m_waitForDataCounter++; + m_waitForDataCond.notify_all(); +} + void DriverStation::GetData() { { // Compute the pressed and released buttons @@ -494,13 +501,7 @@ void DriverStation::GetData() { } } - { - std::scoped_lock waitLock(m_waitForDataMutex); - // Nofify all threads - m_waitForDataCounter++; - m_waitForDataCond.notify_all(); - } - + WakeupWaitForData(); SendMatchData(); } diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp index 950b239e04..a31685182f 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp @@ -30,7 +30,13 @@ void IterativeRobot::StartCompetition() { while (true) { // Wait for driver station data so the loop doesn't hog the CPU DriverStation::GetInstance().WaitForData(); + if (m_exit) break; LoopFunc(); } } + +void IterativeRobot::EndCompetition() { + m_exit = true; + DriverStation::GetInstance().WakeupWaitForData(); +} diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index ffff2ddcb8..a7a069245b 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -43,6 +43,11 @@ void TimedRobot::StartCompetition() { } } +void TimedRobot::EndCompetition() { + int32_t status = 0; + HAL_StopNotifier(m_notifier, &status); +} + units::second_t TimedRobot::GetPeriod() const { return units::second_t(m_period); } diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h index 8abffde766..450870119d 100644 --- a/wpilibc/src/main/native/include/frc/DriverStation.h +++ b/wpilibc/src/main/native/include/frc/DriverStation.h @@ -394,6 +394,11 @@ class DriverStation : public ErrorBase { */ void InTest(bool entering) { m_userInTest = entering; } + /** + * Forces WaitForData() to return immediately. + */ + void WakeupWaitForData(); + protected: /** * Copy data from the DS task for the user. diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h index 8ce63563d0..447c477a1a 100644 --- a/wpilibc/src/main/native/include/frc/IterativeRobot.h +++ b/wpilibc/src/main/native/include/frc/IterativeRobot.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,6 +7,8 @@ #pragma once +#include + #include "frc/IterativeRobotBase.h" namespace frc { @@ -28,9 +30,6 @@ class IterativeRobot : public IterativeRobotBase { IterativeRobot(); virtual ~IterativeRobot() = default; - IterativeRobot(IterativeRobot&&) = default; - IterativeRobot& operator=(IterativeRobot&&) = default; - /** * Provide an alternate "main loop" via StartCompetition(). * @@ -38,6 +37,14 @@ class IterativeRobot : public IterativeRobotBase { * with the DS packets. */ void StartCompetition() override; + + /** + * Ends the main loop in StartCompetition(). + */ + void EndCompetition() override; + + private: + std::atomic m_exit{false}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h index 85a9d12eea..725aa97da0 100644 --- a/wpilibc/src/main/native/include/frc/RobotBase.h +++ b/wpilibc/src/main/native/include/frc/RobotBase.h @@ -7,9 +7,12 @@ #pragma once +#include #include #include +#include +#include #include #include "frc/Base.h" @@ -23,9 +26,13 @@ int RunHALInitialization(); namespace impl { template -void RunRobot() { - static Robot robot; - robot.StartCompetition(); +void RunRobot(wpi::mutex& m, Robot** robot) { + static Robot theRobot; + { + std::scoped_lock lock{m}; + *robot = &theRobot; + } + theRobot.StartCompetition(); } } // namespace impl @@ -36,20 +43,50 @@ int StartRobot() { if (halInit != 0) { return halInit; } + + static wpi::mutex m; + static wpi::condition_variable cv; + static Robot* robot = nullptr; + static bool exited = false; + if (HAL_HasMain()) { - std::thread([] { + std::thread thr([] { try { - impl::RunRobot(); + impl::RunRobot(m, &robot); } catch (...) { HAL_ExitMain(); + { + std::scoped_lock lock{m}; + robot = nullptr; + exited = true; + } + cv.notify_all(); throw; } + HAL_ExitMain(); - }) - .detach(); + { + std::scoped_lock lock{m}; + robot = nullptr; + exited = true; + } + cv.notify_all(); + }); + HAL_RunMain(); + + // signal loop to exit + if (robot) robot->EndCompetition(); + + // prefer to join, but detach to exit if it doesn't exit in a timely manner + using namespace std::chrono_literals; + std::unique_lock lock{m}; + if (cv.wait_for(lock, 1s, [] { return exited; })) + thr.join(); + else + thr.detach(); } else { - impl::RunRobot(); + impl::RunRobot(m, &robot); } return 0; @@ -127,6 +164,8 @@ class RobotBase { virtual void StartCompetition() = 0; + virtual void EndCompetition() = 0; + static constexpr bool IsReal() { #ifdef __FRC_ROBORIO__ return true; diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index 1c8ff801a7..112e2c98a0 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -34,6 +34,11 @@ class TimedRobot : public IterativeRobotBase, public ErrorBase { */ void StartCompetition() override; + /** + * Ends the main loop in StartCompetition(). + */ + void EndCompetition() override; + /** * Get the time period between calls to Periodic() functions. */ diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp index 8495b63a7f..1a856c1e78 100644 --- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp @@ -32,7 +32,7 @@ void Robot::StartCompetition() { // Tell the DS that the robot is ready to be enabled HAL_ObserveUserProgramStarting(); - while (true) { + while (!m_exit) { if (IsDisabled()) { m_ds.InDisabled(true); Disabled(); @@ -61,6 +61,8 @@ void Robot::StartCompetition() { } } +void Robot::EndCompetition() { m_exit = true; } + #ifndef RUNNING_FRC_TESTS int main() { return frc::StartRobot(); } #endif diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h index 4efc087338..0057778c70 100644 --- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h @@ -7,6 +7,8 @@ #pragma once +#include + #include class Robot : public frc::RobotBase { @@ -18,4 +20,8 @@ class Robot : public frc::RobotBase { void Test(); void StartCompetition() override; + void EndCompetition() override; + + private: + std::atomic m_exit{false}; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index 59552011e1..997fe2bc2f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -1008,6 +1008,16 @@ public class DriverStation { m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord()); } + /** + * Forces waitForData() to return immediately. + */ + public void wakeupWaitForData() { + m_waitForDataMutex.lock(); + m_waitForDataCount++; + m_waitForDataCond.signalAll(); + m_waitForDataMutex.unlock(); + } + /** * Copy data from the DS task for the user. If no new data exists, it will just be returned, * otherwise the data will be copied from the DS polling loop. @@ -1061,11 +1071,7 @@ public class DriverStation { m_cacheDataMutex.unlock(); } - m_waitForDataMutex.lock(); - m_waitForDataCount++; - m_waitForDataCond.signalAll(); - m_waitForDataMutex.unlock(); - + wakeupWaitForData(); sendMatchData(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java index 01fa26a0d3..f1baefb25a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java @@ -25,6 +25,7 @@ import edu.wpi.first.hal.HAL; @Deprecated public class IterativeRobot extends IterativeRobotBase { private static final double kPacketPeriod = 0.02; + private volatile boolean m_exit; /** * Create a new IterativeRobot. @@ -49,8 +50,20 @@ public class IterativeRobot extends IterativeRobotBase { while (!Thread.currentThread().isInterrupted()) { // Wait for new data to arrive m_ds.waitForData(); + if (m_exit) { + break; + } loopFunc(); } } + + /** + * Ends the main loop in startCompetition(). + */ + @Override + public void endCompetition() { + m_exit = true; + m_ds.wakeupWaitForData(); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index bc8b8516e2..94ad8845ef 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -12,6 +12,7 @@ import java.io.IOException; import java.io.OutputStream; import java.nio.charset.StandardCharsets; import java.nio.file.Files; +import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import edu.wpi.cscore.CameraServerJNI; @@ -189,6 +190,11 @@ public abstract class RobotBase implements AutoCloseable { */ public abstract void startCompetition(); + /** + * Ends the main loop in startCompetition(). + */ + public abstract void endCompetition(); + @SuppressWarnings("JavadocMethod") public static boolean getBooleanProperty(String name, boolean defaultValue) { String propVal = System.getProperty(name); @@ -204,6 +210,10 @@ public abstract class RobotBase implements AutoCloseable { } } + private static final ReentrantLock m_runMutex = new ReentrantLock(); + private static RobotBase m_robotCopy; + private static boolean m_suppressExitWarning; + /** * Run the robot main loop. */ @@ -232,6 +242,10 @@ public abstract class RobotBase implements AutoCloseable { return; } + m_runMutex.lock(); + m_robotCopy = robot; + m_runMutex.unlock(); + if (isReal()) { try { final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini"); @@ -265,18 +279,32 @@ public abstract class RobotBase implements AutoCloseable { throwable.getStackTrace()); errorOnExit = true; } finally { - // startCompetition never returns unless exception occurs.... - DriverStation.reportWarning("Robots should not quit, but yours did!", false); - if (errorOnExit) { - DriverStation.reportError( - "The startCompetition() method (or methods called by it) should have " - + "handled the exception above.", false); - } else { - DriverStation.reportError("Unexpected return from startCompetition() method.", false); + m_runMutex.lock(); + boolean suppressExitWarning = m_suppressExitWarning; + m_runMutex.unlock(); + if (!suppressExitWarning) { + // startCompetition never returns unless exception occurs.... + DriverStation.reportWarning("Robots should not quit, but yours did!", false); + if (errorOnExit) { + DriverStation.reportError( + "The startCompetition() method (or methods called by it) should have " + + "handled the exception above.", false); + } else { + DriverStation.reportError("Unexpected return from startCompetition() method.", false); + } } } } + /** + * Suppress the "Robots should not quit" message. + */ + public static void suppressExitWarning(boolean value) { + m_runMutex.lock(); + m_suppressExitWarning = value; + m_runMutex.unlock(); + } + /** * Starting point for the applications. */ @@ -299,6 +327,18 @@ public abstract class RobotBase implements AutoCloseable { thread.setDaemon(true); thread.start(); HAL.runMain(); + suppressExitWarning(true); + m_runMutex.lock(); + RobotBase robot = m_robotCopy; + m_runMutex.unlock(); + if (robot != null) { + robot.endCompetition(); + } + try { + thread.join(1000); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } } else { runRobot(robotSupplier); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 9575bf1d96..2167fbc2d6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -82,6 +82,14 @@ public class TimedRobot extends IterativeRobotBase { } } + /** + * Ends the main loop in startCompetition(). + */ + @Override + public void endCompetition() { + NotifierJNI.stopNotifier(m_notifier); + } + /** * Get time period between calls to Periodic() functions. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java index 615af87020..783f76bbc1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java @@ -33,6 +33,8 @@ public class Robot extends RobotBase { public void test() { } + private volatile boolean m_exit; + @SuppressWarnings("PMD.CyclomaticComplexity") @Override public void startCompetition() { @@ -41,7 +43,7 @@ public class Robot extends RobotBase { // Tell the DS that the robot is ready to be enabled HAL.observeUserProgramStarting(); - while (!Thread.currentThread().isInterrupted()) { + while (!Thread.currentThread().isInterrupted() && !m_exit) { if (isDisabled()) { m_ds.InDisabled(true); disabled(); @@ -77,4 +79,9 @@ public class Robot extends RobotBase { } } } + + @Override + public void endCompetition() { + m_exit = true; + } }