Fixes guarantees for waitForData (#252)

As discussed in #234, we now check for the timeout case and handle a
timeout correctly.

wpilibc/sim/src/DriverStation.cpp

@@ -257,13 +257,53 @@ int DriverStation::GetLocation() const {
 
 /**
  * Wait until a new packet comes from the driver station.
+ *
  * This blocks on a semaphore, so the waiting is efficient.
+ *
  * This is a good way to delay processing until there is new driver station data
  * to act on.
  */
-void DriverStation::WaitForData() {
-  std::unique_lock<std::mutex> lock(m_waitForDataMutex);
-  m_waitForDataCond.wait(lock);
+void DriverStation::WaitForData() { WaitForData(0); }
+
+/**
+ * Wait until a new packet comes from the driver station, or wait for a timeout.
+ *
+ * If the timeout is less then or equal to 0, wait indefinitely.
+ *
+ * Timeout is in milliseconds
+ *
+ * This blocks on a semaphore, so the waiting is efficient.
+ *
+ * This is a good way to delay processing until there is new driver station data
+ * to act on.
+ *
+ * @param timeout Timeout time in seconds
+ *
+ * @return true if new data, otherwise false
+ */
+bool DriverStation::WaitForData(double timeout) {
+#if defined(_MSC_VER) && _MSC_VER < 1900
+  auto timeoutTime = std::chrono::steady_clock::now() +
+                     std::chrono::duration<int64_t, std::nano>(
+                         static_cast<int64_t>(timeout * 1e9));
+#else
+  auto timeoutTime =
+      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+#endif
+
+  std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+  while (!m_updatedControlLoopData) {
+    if (timeout > 0) {
+      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+      if (timedOut == std::cv_status::timeout) {
+        return false;
+      }
+    } else {
+      m_waitForDataCond.wait(lock);
+    }
+  }
+  m_updatedControlLoopData = false;
+  return true;
 }
 
 /**
@@ -323,6 +363,10 @@ void DriverStation::stateCallback(const msgs::ConstDriverStationPtr& msg) {
     std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
     *state = *msg;
   }
+  {
+    std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
+    m_updatedControlLoopData = true;
+  }
   m_waitForDataCond.notify_all();
 }