diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index e93a34e363..771d81f958 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -23,7 +23,7 @@ using namespace frc; IterativeRobotBase::IterativeRobotBase(double period) : m_period(period), - m_watchdog(period, [&] { PrintLoopOverrunMessage(); }) {} + m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {} void IterativeRobotBase::RobotInit() { wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp index 2d88afd513..61af552566 100644 --- a/wpilibc/src/main/native/cpp/Watchdog.cpp +++ b/wpilibc/src/main/native/cpp/Watchdog.cpp @@ -7,53 +7,156 @@ #include "frc/Watchdog.h" +#include +#include #include -#include "frc/Timer.h" - using namespace frc; -Watchdog::Watchdog(double timeout, std::function callback) - : m_timeout(timeout), - m_callback(callback), - m_notifier(&Watchdog::TimeoutFunc, this) { - Enable(); +class Watchdog::Thread : public wpi::SafeThread { + public: + template + struct DerefGreater : public std::binary_function { + constexpr bool operator()(const T& lhs, const T& rhs) const { + return *lhs > *rhs; + } + }; + + wpi::PriorityQueue, DerefGreater> + m_watchdogs; + + private: + void Main() override; +}; + +void Watchdog::Thread::Main() { + std::unique_lock lock(m_mutex); + + while (m_active) { + if (m_watchdogs.size() > 0) { + if (m_cond.wait_until(lock, m_watchdogs.top()->m_expirationTime) == + std::cv_status::timeout) { + if (m_watchdogs.size() == 0 || + m_watchdogs.top()->m_expirationTime > hal::fpga_clock::now()) { + continue; + } + + // If the condition variable timed out, that means a Watchdog timeout + // has occurred, so call its timeout function. + auto watchdog = m_watchdogs.top(); + m_watchdogs.pop(); + + wpi::outs() << "Watchdog not fed within " + << wpi::format("%.6f", watchdog->m_timeout.count() / 1.0e6) + << "s\n"; + lock.unlock(); + watchdog->m_callback(); + lock.lock(); + watchdog->m_isExpired = true; + } + // Otherwise, a Watchdog removed itself from the queue (it notifies the + // scheduler of this) or a spurious wakeup occurred, so just rewait with + // the soonest watchdog timeout. + } else { + m_cond.wait(lock, [&] { return m_watchdogs.size() > 0 || !m_active; }); + } + } } +Watchdog::Watchdog(double timeout, std::function callback) + : m_timeout(static_cast(timeout * 1.0e6)), + m_callback(callback), + m_owner(&GetThreadOwner()) {} + +Watchdog::~Watchdog() { Disable(); } + double Watchdog::GetTime() const { - return Timer::GetFPGATimestamp() - m_startTime; + return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6; } -bool Watchdog::IsExpired() const { return m_isExpired; } +void Watchdog::SetTimeout(double timeout) { + m_startTime = hal::fpga_clock::now(); + m_epochs.clear(); + + // Locks mutex + auto thr = m_owner->GetThread(); + if (!thr) return; + + m_timeout = std::chrono::microseconds(static_cast(timeout * 1.0e6)); + m_isExpired = false; + + thr->m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + thr->m_watchdogs.emplace(this); + thr->m_cond.notify_all(); +} + +double Watchdog::GetTimeout() const { + // Locks mutex + auto thr = m_owner->GetThread(); + + return m_timeout.count() / 1.0e6; +} + +bool Watchdog::IsExpired() const { + // Locks mutex + auto thr = m_owner->GetThread(); + + return m_isExpired; +} void Watchdog::AddEpoch(wpi::StringRef epochName) { - double currentTime = Timer::GetFPGATimestamp(); + auto currentTime = hal::fpga_clock::now(); m_epochs[epochName] = currentTime - m_startTime; m_startTime = currentTime; } void Watchdog::PrintEpochs() { for (const auto& epoch : m_epochs) { - wpi::outs() << "\t" << epoch.getKey() << ": " << epoch.getValue() << "s\n"; + wpi::outs() << '\t' << epoch.getKey() << ": " + << wpi::format("%.6f", epoch.getValue().count() / 1.0e6) + << "s\n"; } } void Watchdog::Reset() { Enable(); } void Watchdog::Enable() { - m_startTime = Timer::GetFPGATimestamp(); - m_isExpired = false; + m_startTime = hal::fpga_clock::now(); m_epochs.clear(); - m_notifier.StartPeriodic(m_timeout); + + // Locks mutex + auto thr = m_owner->GetThread(); + if (!thr) return; + + m_isExpired = false; + + thr->m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + thr->m_watchdogs.emplace(this); + thr->m_cond.notify_all(); } -void Watchdog::Disable() { m_notifier.Stop(); } +void Watchdog::Disable() { + // Locks mutex + auto thr = m_owner->GetThread(); + if (!thr) return; -void Watchdog::TimeoutFunc() { - if (!m_isExpired) { - wpi::outs() << "Watchdog not fed after " << m_timeout << "s\n"; - m_callback(); - m_isExpired = true; - Disable(); - } + m_isExpired = false; + + thr->m_watchdogs.remove(this); + thr->m_cond.notify_all(); +} + +bool Watchdog::operator>(const Watchdog& rhs) { + return m_expirationTime > rhs.m_expirationTime; +} + +wpi::SafeThreadOwner& Watchdog::GetThreadOwner() { + static wpi::SafeThreadOwner inst = [] { + wpi::SafeThreadOwner inst; + inst.Start(); + return inst; + }(); + return inst; } diff --git a/wpilibc/src/main/native/include/frc/Watchdog.h b/wpilibc/src/main/native/include/frc/Watchdog.h index d32fa11322..2f24b76345 100644 --- a/wpilibc/src/main/native/include/frc/Watchdog.h +++ b/wpilibc/src/main/native/include/frc/Watchdog.h @@ -7,13 +7,14 @@ #pragma once +#include #include +#include +#include #include #include -#include "frc/Notifier.h" - namespace frc { /** @@ -30,19 +31,35 @@ class Watchdog { /** * Watchdog constructor. * - * @param timeout The watchdog's timeout in seconds. + * @param timeout The watchdog's timeout in seconds with microsecond + * resolution. * @param callback This function is called when the timeout expires. */ - explicit Watchdog(double timeout, std::function callback = [] {}); + Watchdog(double timeout, std::function callback); + + ~Watchdog(); Watchdog(Watchdog&&) = default; Watchdog& operator=(Watchdog&&) = default; /** - * Get the time in seconds since the watchdog was last fed. + * Returns the time in seconds since the watchdog was last fed. */ double GetTime() const; + /** + * Sets the watchdog's timeout. + * + * @param timeout The watchdog's timeout in seconds with microsecond + * resolution. + */ + void SetTimeout(double timeout); + + /** + * Returns the watchdog's timeout in seconds. + */ + double GetTimeout() const; + /** * Returns true if the watchdog timer has expired. */ @@ -76,20 +93,25 @@ class Watchdog { void Enable(); /** - * Disable the watchdog. + * Disables the watchdog timer. */ void Disable(); private: - double m_timeout; + hal::fpga_clock::time_point m_startTime; + std::chrono::microseconds m_timeout; + hal::fpga_clock::time_point m_expirationTime; std::function m_callback; - Notifier m_notifier; - double m_startTime = 0.0; - wpi::StringMap m_epochs; + wpi::StringMap m_epochs; bool m_isExpired = false; - void TimeoutFunc(); + class Thread; + wpi::SafeThreadOwner* m_owner; + + bool operator>(const Watchdog& rhs); + + static wpi::SafeThreadOwner& GetThreadOwner(); }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/wpilibc/src/test/native/cpp/WatchdogTest.cpp new file mode 100644 index 0000000000..e85347df20 --- /dev/null +++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp @@ -0,0 +1,144 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 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. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/Watchdog.h" // NOLINT(build/include_order) + +#include + +#include + +#include + +#include "gtest/gtest.h" + +using namespace frc; + +TEST(WatchdogTest, EnableDisable) { + uint32_t watchdogCounter = 0; + + Watchdog watchdog(0.4, [&] { watchdogCounter++; }); + + wpi::outs() << "Run 1\n"; + watchdog.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.Disable(); + + EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early"; + + wpi::outs() << "Run 2\n"; + watchdogCounter = 0; + watchdog.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(600)); + watchdog.Disable(); + + EXPECT_EQ(1u, watchdogCounter) + << "Watchdog either didn't trigger or triggered more than once"; + + wpi::outs() << "Run 3\n"; + watchdogCounter = 0; + watchdog.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + watchdog.Disable(); + + EXPECT_EQ(1u, watchdogCounter) + << "Watchdog either didn't trigger or triggered more than once"; +} + +TEST(WatchdogTest, Reset) { + uint32_t watchdogCounter = 0; + + Watchdog watchdog(0.4, [&] { watchdogCounter++; }); + + watchdog.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.Reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.Disable(); + + EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early"; +} + +TEST(WatchdogTest, SetTimeout) { + uint32_t watchdogCounter = 0; + + Watchdog watchdog(1.0, [&] { watchdogCounter++; }); + + watchdog.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.SetTimeout(0.2); + + EXPECT_EQ(0.2, watchdog.GetTimeout()); + EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early"; + + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + watchdog.Disable(); + + EXPECT_EQ(1u, watchdogCounter) + << "Watchdog either didn't trigger or triggered more than once"; +} + +TEST(WatchdogTest, IsExpired) { + Watchdog watchdog(0.2, [] {}); + watchdog.Enable(); + + EXPECT_FALSE(watchdog.IsExpired()); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + EXPECT_TRUE(watchdog.IsExpired()); +} + +TEST(WatchdogTest, Epochs) { + uint32_t watchdogCounter = 0; + + Watchdog watchdog(0.4, [&] { watchdogCounter++; }); + + wpi::outs() << "Run 1\n"; + watchdog.Enable(); + watchdog.AddEpoch("Epoch 1"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + watchdog.AddEpoch("Epoch 2"); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + watchdog.AddEpoch("Epoch 3"); + watchdog.Disable(); + watchdog.PrintEpochs(); + + EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early"; + + wpi::outs() << "Run 2\n"; + watchdog.Enable(); + watchdog.AddEpoch("Epoch 1"); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.Reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + watchdog.AddEpoch("Epoch 2"); + watchdog.Disable(); + watchdog.PrintEpochs(); + + EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early"; +} + +TEST(WatchdogTest, MultiWatchdog) { + uint32_t watchdogCounter1 = 0; + uint32_t watchdogCounter2 = 0; + + Watchdog watchdog1(0.2, [&] { watchdogCounter1++; }); + Watchdog watchdog2(0.6, [&] { watchdogCounter2++; }); + + watchdog2.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + EXPECT_EQ(0u, watchdogCounter1) << "Watchdog triggered early"; + EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early"; + + // Sleep enough such that only the watchdog enabled later times out first + watchdog1.Enable(); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + watchdog1.Disable(); + watchdog2.Disable(); + + EXPECT_EQ(1u, watchdogCounter1) + << "Watchdog either didn't trigger or triggered more than once"; + EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early"; +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index fc0d8417c0..998836e479 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -7,8 +7,13 @@ package edu.wpi.first.wpilibj; +import java.io.Closeable; import java.util.HashMap; import java.util.Map; +import java.util.PriorityQueue; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.ReentrantLock; /** * A class that's a wrapper around a watchdog timer. @@ -18,51 +23,106 @@ import java.util.Map; * *

The watchdog is initialized disabled, so the user needs to call enable() before use. */ -public class Watchdog { - private double m_timeout; - private Runnable m_callback; - private Notifier m_notifier; +public class Watchdog implements Closeable, Comparable { + private long m_startTime; // us + private long m_timeout; // us + private long m_expirationTime; // us + private final Runnable m_callback; - private double m_startTime; @SuppressWarnings("PMD.UseConcurrentHashMap") - private final Map m_epochs = new HashMap<>(); + private final Map m_epochs = new HashMap<>(); boolean m_isExpired; - /** - * Watchdog constructor. - * - * @param timeout The watchdog's timeout in seconds. - */ - public Watchdog(double timeout) { - this(timeout, () -> { - }); + static { + startDaemonThread(() -> schedulerFunc()); } + private static final PriorityQueue m_watchdogs = new PriorityQueue<>(); + private static ReentrantLock m_queueMutex = new ReentrantLock(); + private static Condition m_schedulerWaiter = m_queueMutex.newCondition(); + /** * Watchdog constructor. * - * @param timeout The watchdog's timeout in seconds. + * @param timeout The watchdog's timeout in seconds with microsecond resolution. * @param callback This function is called when the timeout expires. */ public Watchdog(double timeout, Runnable callback) { - m_timeout = timeout; + m_timeout = (long) (timeout * 1.0e6); m_callback = callback; - m_notifier = new Notifier(this::timeoutFunc); - enable(); + } + + @Override + public void close() { + disable(); + } + + @Override + public int compareTo(Watchdog rhs) { + // Elements with sooner expiration times are sorted as lesser. The head of + // Java's PriorityQueue is the least element. + if (m_expirationTime < rhs.m_expirationTime) { + return -1; + } else if (m_expirationTime > rhs.m_expirationTime) { + return 1; + } else { + return 0; + } } /** * Get the time in seconds since the watchdog was last fed. */ public double getTime() { - return Timer.getFPGATimestamp() - m_startTime; + return (RobotController.getFPGATime() - m_startTime) / 1.0e6; + } + + /** + * Sets the watchdog's timeout. + * + * @param timeout The watchdog's timeout in seconds with microsecond + * resolution. + */ + public void setTimeout(double timeout) { + m_startTime = RobotController.getFPGATime(); + m_epochs.clear(); + + m_queueMutex.lock(); + try { + m_timeout = (long) (timeout * 1.0e6); + m_isExpired = false; + + m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + m_watchdogs.add(this); + m_schedulerWaiter.signalAll(); + } finally { + m_queueMutex.unlock(); + } + } + + /** + * Returns the watchdog's timeout in seconds. + */ + public double getTimeout() { + m_queueMutex.lock(); + try { + return m_timeout / 1.0e6; + } finally { + m_queueMutex.unlock(); + } } /** * Returns true if the watchdog timer has expired. */ public boolean isExpired() { - return m_isExpired; + m_queueMutex.lock(); + try { + return m_isExpired; + } finally { + m_queueMutex.unlock(); + } } /** @@ -74,7 +134,7 @@ public class Watchdog { * @param epochName The name to associate with the epoch. */ public void addEpoch(String epochName) { - double currentTime = Timer.getFPGATimestamp(); + long currentTime = RobotController.getFPGATime(); m_epochs.put(epochName, currentTime - m_startTime); m_startTime = currentTime; } @@ -84,7 +144,7 @@ public class Watchdog { */ public void printEpochs() { m_epochs.forEach((key, value) -> { - System.out.println("\t" + key + ": " + value + "s"); + System.out.format("\t" + key + ": %.6fs\n", value / 1.0e6); }); } @@ -101,25 +161,100 @@ public class Watchdog { * Enables the watchdog timer. */ public void enable() { - m_startTime = Timer.getFPGATimestamp(); - m_isExpired = false; + m_startTime = RobotController.getFPGATime(); m_epochs.clear(); - m_notifier.startPeriodic(m_timeout); + + m_queueMutex.lock(); + try { + m_isExpired = false; + + m_watchdogs.remove(this); + m_expirationTime = m_startTime + m_timeout; + m_watchdogs.add(this); + m_schedulerWaiter.signalAll(); + } finally { + m_queueMutex.unlock(); + } } /** * Disable the watchdog. */ public void disable() { - m_notifier.stop(); - } + m_queueMutex.lock(); + try { + m_isExpired = false; - private void timeoutFunc() { - if (!m_isExpired) { - System.out.println("Watchdog not fed after " + m_timeout + "s"); - m_callback.run(); - m_isExpired = true; - disable(); + m_watchdogs.remove(this); + m_schedulerWaiter.signalAll(); + } finally { + m_queueMutex.unlock(); } } + + private static Thread startDaemonThread(Runnable target) { + Thread inst = new Thread(target); + inst.setDaemon(true); + inst.start(); + return inst; + } + + + private static void schedulerFunc() { + m_queueMutex.lock(); + + try { + while (true) { + if (m_watchdogs.size() > 0) { + boolean timedOut = !awaitUntil(m_schedulerWaiter, m_watchdogs.peek().m_expirationTime); + if (timedOut) { + if (m_watchdogs.size() == 0 || m_watchdogs.peek().m_expirationTime + > RobotController.getFPGATime()) { + continue; + } + + // If the condition variable timed out, that means a Watchdog timeout + // has occurred, so call its timeout function. + Watchdog watchdog = m_watchdogs.poll(); + + System.out.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout / 1.0e6); + m_queueMutex.unlock(); + watchdog.m_callback.run(); + m_queueMutex.lock(); + watchdog.m_isExpired = true; + } + // Otherwise, a Watchdog removed itself from the queue (it notifies + // the scheduler of this) or a spurious wakeup occurred, so just + // rewait with the soonest watchdog timeout. + } else { + while (m_watchdogs.size() == 0) { + m_schedulerWaiter.awaitUninterruptibly(); + } + } + } + } finally { + m_queueMutex.unlock(); + } + } + + /** + * Wrapper emulating functionality of C++'s std::condition_variable::wait_until(). + * + * @param cond The condition variable on which to wait. + * @param time The time at which to stop waiting. + * @return False if the deadline has elapsed upon return, else true. + */ + private static boolean awaitUntil(Condition cond, long time) { + long delta = time - RobotController.getFPGATime(); + try { + if (delta > 0) { + return cond.await(delta, TimeUnit.MICROSECONDS); + } + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + ex.printStackTrace(); + } + + return true; + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java new file mode 100644 index 0000000000..75a5df6437 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java @@ -0,0 +1,219 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import java.util.concurrent.atomic.AtomicInteger; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class WatchdogTest { + @Test + void enableDisableTest() { + final AtomicInteger watchdogCounter = new AtomicInteger(0); + + final Watchdog watchdog = new Watchdog(0.4, () -> { + watchdogCounter.addAndGet(1); + }); + + // Run 1 + watchdog.enable(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.disable(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + + // Run 2 + watchdogCounter.set(0); + watchdog.enable(); + try { + Thread.sleep(600); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.disable(); + + assertEquals(1, watchdogCounter.get(), + "Watchdog either didn't trigger or triggered more than once"); + + // Run 3 + watchdogCounter.set(0); + watchdog.enable(); + try { + Thread.sleep(1000); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.disable(); + + assertEquals(1, watchdogCounter.get(), + "Watchdog either didn't trigger or triggered more than once"); + } + + @Test + void resetTest() { + final AtomicInteger watchdogCounter = new AtomicInteger(0); + + final Watchdog watchdog = new Watchdog(0.4, () -> { + watchdogCounter.addAndGet(1); + }); + + watchdog.enable(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.reset(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.disable(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + } + + @Test + void setTimeoutTest() { + final AtomicInteger watchdogCounter = new AtomicInteger(0); + + final Watchdog watchdog = new Watchdog(1.0, () -> { + watchdogCounter.addAndGet(1); + }); + + watchdog.enable(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.setTimeout(0.2); + + assertEquals(0.2, watchdog.getTimeout()); + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + + try { + Thread.sleep(300); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.disable(); + + assertEquals(1, watchdogCounter.get(), + "Watchdog either didn't trigger or triggered more than once"); + } + + @Test + void isExpiredTest() { + final Watchdog watchdog = new Watchdog(0.2, () -> { + }); + watchdog.enable(); + + assertFalse(watchdog.isExpired()); + try { + Thread.sleep(300); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + assertTrue(watchdog.isExpired()); + } + + @Test + void epochsTest() { + final AtomicInteger watchdogCounter = new AtomicInteger(0); + + final Watchdog watchdog = new Watchdog(0.4, () -> { + watchdogCounter.addAndGet(1); + }); + + // Run 1 + watchdog.enable(); + watchdog.addEpoch("Epoch 1"); + try { + Thread.sleep(100); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.addEpoch("Epoch 2"); + try { + Thread.sleep(100); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.addEpoch("Epoch 3"); + watchdog.disable(); + watchdog.printEpochs(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + + // Run 2 + watchdog.enable(); + watchdog.addEpoch("Epoch 1"); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.reset(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog.addEpoch("Epoch 2"); + watchdog.disable(); + watchdog.printEpochs(); + + assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); + } + + @Test + void multiWatchdogTest() { + final AtomicInteger watchdogCounter1 = new AtomicInteger(0); + final AtomicInteger watchdogCounter2 = new AtomicInteger(0); + + final Watchdog watchdog1 = new Watchdog(0.2, () -> { + watchdogCounter1.addAndGet(1); + }); + final Watchdog watchdog2 = new Watchdog(0.6, () -> { + watchdogCounter2.addAndGet(1); + }); + + watchdog2.enable(); + try { + Thread.sleep(200); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early"); + assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early"); + + // Sleep enough such that only the watchdog enabled later times out first + watchdog1.enable(); + try { + Thread.sleep(300); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + watchdog1.disable(); + watchdog2.disable(); + + assertEquals(1, watchdogCounter1.get(), + "Watchdog either didn't trigger or triggered more than once"); + assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early"); + } +} diff --git a/wpiutil/src/main/native/include/wpi/PriorityQueue.h b/wpiutil/src/main/native/include/wpi/PriorityQueue.h new file mode 100644 index 0000000000..37585a3091 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/PriorityQueue.h @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 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. */ +/*----------------------------------------------------------------------------*/ + +#ifndef WPIUTIL_WPI_PRIORITYQUEUE_H_ +#define WPIUTIL_WPI_PRIORITYQUEUE_H_ + +#include +#include +#include +#include + +namespace wpi { + +/** + * This class adds a method for removing all elements from the priority queue + * matching the given value. + */ +template , + class Compare = std::less> +class PriorityQueue : public std::priority_queue { + public: + bool remove(const T& value) { + auto it = std::find(this->c.begin(), this->c.end(), value); + if (it != this->c.end()) { + this->c.erase(it); + std::make_heap(this->c.begin(), this->c.end(), this->comp); + return true; + } else { + return false; + } + } +}; + +} // namespace wpi + +#endif // WPIUTIL_WPI_PRIORITYQUEUE_H_