Make Watchdog use single thread dispatch (#1347)

Notifier has one thread per instance because the callbacks must be
asynchronous. Watchdog callbacks can be synchronous, so this overhead
can be done away with via a scheduler thread akin to what the HAL
Notifier does.

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";

wpilibc/src/main/native/cpp/Watchdog.cpp

@@ -7,53 +7,156 @@
 
 #include "frc/Watchdog.h"
 
+#include <wpi/Format.h>
+#include <wpi/PriorityQueue.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/Timer.h"
-
 using namespace frc;
 
-Watchdog::Watchdog(double timeout, std::function<void()> callback)
-    : m_timeout(timeout),
-      m_callback(callback),
-      m_notifier(&Watchdog::TimeoutFunc, this) {
-  Enable();
+class Watchdog::Thread : public wpi::SafeThread {
+ public:
+  template <typename T>
+  struct DerefGreater : public std::binary_function<T, T, bool> {
+    constexpr bool operator()(const T& lhs, const T& rhs) const {
+      return *lhs > *rhs;
+    }
+  };
+
+  wpi::PriorityQueue<Watchdog*, std::vector<Watchdog*>, DerefGreater<Watchdog*>>
+      m_watchdogs;
+
+ private:
+  void Main() override;
+};
+
+void Watchdog::Thread::Main() {
+  std::unique_lock<wpi::mutex> 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<void()> callback)
+    : m_timeout(static_cast<int64_t>(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<int64_t>(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::Thread>& Watchdog::GetThreadOwner() {
+  static wpi::SafeThreadOwner<Thread> inst = [] {
+    wpi::SafeThreadOwner<Watchdog::Thread> inst;
+    inst.Start();
+    return inst;
+  }();
+  return inst;
 }

wpilibc/src/main/native/include/frc/Watchdog.h

@@ -7,13 +7,14 @@
 
 #pragma once
 
+#include <chrono>
 #include <functional>
 
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/SafeThread.h>
 #include <wpi/StringMap.h>
 #include <wpi/StringRef.h>
 
-#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<void()> callback = [] {});
+  Watchdog(double timeout, std::function<void()> 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<void()> m_callback;
-  Notifier m_notifier;
 
-  double m_startTime = 0.0;
-  wpi::StringMap<double> m_epochs;
+  wpi::StringMap<std::chrono::microseconds> m_epochs;
   bool m_isExpired = false;
 
-  void TimeoutFunc();
+  class Thread;
+  wpi::SafeThreadOwner<Thread>* m_owner;
+
+  bool operator>(const Watchdog& rhs);
+
+  static wpi::SafeThreadOwner<Thread>& GetThreadOwner();
 };
 
 }  // namespace frc

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 <stdint.h>
+
+#include <thread>
+
+#include <wpi/raw_ostream.h>
+
+#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";
+}

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;
  *
  * <p>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<Watchdog> {
+  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<String, Double> m_epochs = new HashMap<>();
+  private final Map<String, Long> 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<Watchdog> 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;
+  }
 }

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");
+  }
+}

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 <algorithm>
+#include <functional>
+#include <queue>
+#include <vector>
+
+namespace wpi {
+
+/**
+ * This class adds a method for removing all elements from the priority queue
+ * matching the given value.
+ */
+template <class T, class Container = std::vector<T>,
+          class Compare = std::less<typename Container::value_type>>
+class PriorityQueue : public std::priority_queue<T, Container, Compare> {
+ 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_