From 4896a77c869ab505c9e3583af5b482753bc3751c Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 9 Oct 2016 11:46:01 -0700 Subject: [PATCH] Fixes guarantees for waitForData (#252) As discussed in #234, we now check for the timeout case and handle a timeout correctly. --- wpilibc/athena/include/DriverStation.h | 1 + wpilibc/athena/src/DriverStation.cpp | 41 +++++++++++- wpilibc/sim/include/DriverStation.h | 2 + wpilibc/sim/src/DriverStation.cpp | 50 +++++++++++++- .../edu/wpi/first/wpilibj/DriverStation.java | 53 ++++++++++++--- .../edu/wpi/first/wpilibj/DriverStation.java | 65 +++++++++++++++---- 6 files changed, 187 insertions(+), 25 deletions(-) diff --git a/wpilibc/athena/include/DriverStation.h b/wpilibc/athena/include/DriverStation.h index 14f7755e50..338503381e 100644 --- a/wpilibc/athena/include/DriverStation.h +++ b/wpilibc/athena/include/DriverStation.h @@ -68,6 +68,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { Alliance GetAlliance() const; int GetLocation() const; void WaitForData(); + bool WaitForData(double timeout); double GetMatchTime() const; float GetBatteryVoltage() const; diff --git a/wpilibc/athena/src/DriverStation.cpp b/wpilibc/athena/src/DriverStation.cpp index 7761b81873..980e95c83b 100644 --- a/wpilibc/athena/src/DriverStation.cpp +++ b/wpilibc/athena/src/DriverStation.cpp @@ -7,6 +7,8 @@ #include "DriverStation.h" +#include + #include "AnalogInput.h" #include "FRC_NetworkCommunication/FRCComm.h" #include "HAL/cpp/Log.h" @@ -445,12 +447,47 @@ int DriverStation::GetLocation() const { * This is a good way to delay processing until there is new driver station data * to act on. */ -void DriverStation::WaitForData() { +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( + static_cast(timeout * 1e9)); +#else + auto timeoutTime = + std::chrono::steady_clock::now() + std::chrono::duration(timeout); +#endif + std::unique_lock lock(m_waitForDataMutex); while (!m_updatedControlLoopData) { - m_waitForDataCond.wait(lock); + 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; } /** diff --git a/wpilibc/sim/include/DriverStation.h b/wpilibc/sim/include/DriverStation.h index 2a9f1230c5..1c2f395baf 100644 --- a/wpilibc/sim/include/DriverStation.h +++ b/wpilibc/sim/include/DriverStation.h @@ -68,6 +68,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { Alliance GetAlliance() const; int GetLocation() const; void WaitForData(); + bool WaitForData(double timeout); double GetMatchTime() const; float GetBatteryVoltage() const; uint16_t GetTeamNumber() const; @@ -128,6 +129,7 @@ class DriverStation : public SensorBase, public RobotStateInterface { int m_digitalOut = 0; std::condition_variable m_waitForDataCond; std::mutex m_waitForDataMutex; + bool m_updatedControlLoopData = false; mutable std::recursive_mutex m_stateMutex; std::recursive_mutex m_joystickMutex; double m_approxMatchTimeOffset = 0; diff --git a/wpilibc/sim/src/DriverStation.cpp b/wpilibc/sim/src/DriverStation.cpp index 2cb22c5ba1..2373888a73 100644 --- a/wpilibc/sim/src/DriverStation.cpp +++ b/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 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( + static_cast(timeout * 1e9)); +#else + auto timeoutTime = + std::chrono::steady_clock::now() + std::chrono::duration(timeout); +#endif + + std::unique_lock 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 lock(m_stateMutex); *state = *msg; } + { + std::lock_guard lock(m_waitForDataMutex); + m_updatedControlLoopData = true; + } m_waitForDataCond.notify_all(); } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java index 70e6d0a913..fbd701abab 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java @@ -8,6 +8,10 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; import edu.wpi.first.wpilibj.hal.AllianceStationID; import edu.wpi.first.wpilibj.hal.ControlWord; @@ -89,7 +93,9 @@ public class DriverStation implements RobotState.Interface { private int[][] m_joystickAxisType = new int[kJoystickPorts][HAL.kMaxJoystickAxes]; private Thread m_thread; - private final Object m_dataSem; + private final Lock m_dataMutex; + private final Condition m_dataCond; + private final Object m_newControlDataMutex; private final Object m_joystickMutex; private volatile boolean m_threadKeepAlive = true; @@ -121,7 +127,8 @@ public class DriverStation implements RobotState.Interface { * variable. */ private DriverStation() { - m_dataSem = new Object(); + m_dataMutex = new ReentrantLock(); + m_dataCond = m_dataMutex.newCondition(); m_joystickMutex = new Object(); m_newControlDataMutex = new Object(); for (int i = 0; i < kJoystickPorts; i++) { @@ -636,18 +643,43 @@ public class DriverStation implements RobotState.Interface { * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data * only. * - * @param timeout The maximum time in milliseconds to wait. + * @param timeout The maximum time in seconds to wait. + * @return true if there is new data, otherwise false */ - public void waitForData(long timeout) { - synchronized (m_dataSem) { + public boolean waitForData(double timeout) { + long startTime = Utility.getFPGATime(); + long timeoutMicros = (long)(timeout * 1000000); + m_dataMutex.lock(); + try { try { while (!m_updatedControlLoopData) { - m_dataSem.wait(timeout); + if (timeout > 0) { + long now = Utility.getFPGATime(); + if (now < startTime + timeoutMicros) { + // We still have time to wait + boolean signaled = m_dataCond.await(startTime + timeoutMicros - now, + TimeUnit.MICROSECONDS); + if (!signaled) { + // Return false if a timeout happened + return false; + } + } else { + // Time has elapsed. + return false; + } + } else { + m_dataCond.await(); + } } m_updatedControlLoopData = false; + // Return true if we have received a proper signal + return true; } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); + // return false on a thread interrupt + return false; } + } finally { + m_dataMutex.unlock(); } } @@ -787,9 +819,12 @@ public class DriverStation implements RobotState.Interface { while (m_threadKeepAlive) { HAL.waitForDSData(); getData(); - synchronized (m_dataSem) { + m_dataMutex.lock(); + try { m_updatedControlLoopData = true; - m_dataSem.notifyAll(); + m_dataCond.signalAll(); + } finally { + m_dataMutex.unlock(); } if (++safetyCounter >= 4) { MotorSafetyHelper.checkMotors(); diff --git a/wpilibj/src/sim/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/DriverStation.java index f752c8156a..6252a82b7f 100644 --- a/wpilibj/src/sim/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/DriverStation.java @@ -9,6 +9,11 @@ package edu.wpi.first.wpilibj; import org.gazebosim.transport.SubscriberCallback; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + import edu.wpi.first.wpilibj.simulation.MainNode; import gazebo.msgs.GzDriverStation; import gazebo.msgs.GzDriverStation.DriverStation.State; @@ -76,7 +81,9 @@ public class DriverStation implements RobotState.Interface { private static DriverStation instance = new DriverStation(); - private final Object m_dataSem; + private final Lock m_dataMutex; + private final Condition m_dataCond; + private boolean m_updatedControlLoopData; private boolean m_userInDisabled = false; private boolean m_userInAutonomous = false; private boolean m_userInTeleop = false; @@ -101,7 +108,8 @@ public class DriverStation implements RobotState.Interface { * variable. */ protected DriverStation() { - m_dataSem = new Object(); + m_dataMutex = new ReentrantLock(); + m_dataCond = m_dataMutex.newCondition(); MainNode.subscribe("ds/state", GzDriverStation.DriverStation.getDefaultInstance(), new SubscriberCallback() { @@ -109,8 +117,12 @@ public class DriverStation implements RobotState.Interface { public void callback(GzDriverStation.DriverStation msg) { state = msg; m_newControlData = true; - synchronized (m_dataSem) { - m_dataSem.notifyAll(); + m_dataMutex.lock(); + try { + m_updatedControlLoopData = true; + m_dataCond.signalAll(); + } finally { + m_dataMutex.unlock(); } } } @@ -122,8 +134,11 @@ public class DriverStation implements RobotState.Interface { new SubscriberCallback() { @Override public void callback(Joystick msg) { - synchronized (m_dataSem) { + m_dataMutex.lock(); + try { joysticks[j] = msg; + } finally { + m_dataMutex.unlock(); } } } @@ -139,18 +154,46 @@ public class DriverStation implements RobotState.Interface { } /** - * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data + * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data * only. * - * @param timeout The maximum time in milliseconds to wait. + * @param timeout The maximum time in seconds to wait. + * @return true if there is new data, otherwise false */ - public void waitForData(long timeout) { - synchronized (m_dataSem) { + public boolean waitForData(double timeout) { + long startTime = (long)(Timer.getFPGATimestamp() * 1000000); + long timeoutMicros = (long)(timeout * 1000000); + m_dataMutex.lock(); + try { try { - m_dataSem.wait(timeout); + while (!m_updatedControlLoopData) { + if (timeout > 0) { + long now = (long)(Timer.getFPGATimestamp() * 1000000); + if (now < startTime + timeoutMicros) { + // We still have time to wait + boolean signaled = m_dataCond.await(startTime + timeoutMicros - now, + TimeUnit.MICROSECONDS); + if (!signaled) { + // Return false if a timeout happened + return false; + } + } else { + // Time has elapsed. + return false; + } + } else { + m_dataCond.await(); + } + } + m_updatedControlLoopData = false; + // Return true if we have received a proper signal + return true; } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); + // return false on a thread interrupt + return false; } + } finally { + m_dataMutex.unlock(); } }