Skip to content

Commit

Permalink
Merge pull request #124 from DeepBlueRobotics/fix-tests-not-repeatable
Browse files Browse the repository at this point in the history 
Make tests repeatable and more accurate.
  • Loading branch information
@brettle
brettle authored Jul 20, 2024
2 parents 5be9c56 + d4079a0 commit fca5060
Show file tree
Hide file tree
Showing 2 changed files with 98 additions and 54 deletions.
80 changes: 56 additions & 24 deletions plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,8 +86,7 @@ public class WebotsSimulator implements AutoCloseable {
private final StringPublisher simModePublisher;

// We'll use this to run all NT listener callbacks sequentially on a single separate thread.
private final ExecutorService listenerCallbackExecutor =
Executors.newSingleThreadExecutor();
private ExecutorService listenerCallbackExecutor;

// Use these to control NetworkTables logging.
// - ntLoglevel = 0 means no NT logging
Expand Down Expand Up @@ -142,6 +141,13 @@ public WebotsSimulator setMaxJitterSecs(double maxJitterSecs) {
}
}

private void runOnExecutorThread(Runnable r) {
if (listenerCallbackExecutor.isShutdown()) {
return;
}
listenerCallbackExecutor.execute(r);
}

private static synchronized void acquireFileLock()
throws InterruptedException, IOException {
if (fileLock == null) {
Expand Down Expand Up @@ -326,7 +332,7 @@ private WebotsSimulator waitForUserToStart(String worldFileAbsPath)
inst.addListener(statusSubscriber,
EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> {
final var eventValue = event.valueData.value.getString();
listenerCallbackExecutor.execute(() -> {
runOnExecutorThread(() -> {
LOG.log(Level.DEBUG, "In listener, status = {0}",
eventValue);
if (!eventValue.equals(
Expand Down Expand Up @@ -433,7 +439,7 @@ private void startTimeSync() {
LOG.log(Level.DEBUG,
"In simTimeSec value changed callback");
final var eventValue = value.getDouble();
listenerCallbackExecutor.execute(() -> {
runOnExecutorThread(() -> {
LOG.log(Level.DEBUG, "Got simTimeSec value of {0}",
eventValue);
if (eventValue < 0.0) {
Expand Down Expand Up @@ -491,7 +497,7 @@ private void startTimeSync() {

waitForHALSimWSConnection();

listenerCallbackExecutor.execute(() -> {
runOnExecutorThread(() -> {
simTimeSec = 0.0;
sendRobotTime();
});
Expand All @@ -506,14 +512,12 @@ private void waitForHALSimWSConnection() {
inst.addListener(statusSubscriber,
EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> {
final var eventValue = event.valueData.value.getString();
listenerCallbackExecutor.execute(() -> {
LOG.log(Level.DEBUG, "In listener, status = {0}",
eventValue);
if (!eventValue.equals(
NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE))
return;
halSimWSConnected.complete(null);
});
LOG.log(Level.DEBUG, "In listener, status = {0}",
eventValue);
if (!eventValue.equals(
NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE))
return;
halSimWSConnected.complete(null);
});

requestPublisher.set(NTConstants.REQUEST_HALSIMWS_CONNECTION_VERB);
Expand Down Expand Up @@ -790,6 +794,8 @@ private void endCompetition(TimedRobot robot) {
public void run() throws InstantiationException, IllegalAccessException,
InvocationTargetException, NoSuchMethodException,
SecurityException, TimeoutException {
listenerCallbackExecutor = Executors.newSingleThreadExecutor();

endCompetitionCalled = false;
// HAL must be initialized or SmartDashboard might not work.
HAL.initialize(500, 0);
Expand Down Expand Up @@ -821,20 +827,31 @@ public void run() throws InstantiationException, IllegalAccessException,
});
}

// Run the onInited callbacks once.
// Note: the offset is -period so they are run before other WPILib periodic methods
// Schedule the onInited callbacks to be run once. Note: the offset is -period so they
// are run before other WPILib periodic methods the 1e-6 is so that it isn't scheduled
// for a timestamp of exactly 0.0 because that is a special value that causes
// startCompetition() to end.
robot.addPeriodic(this::runRobotInitedCallbacksOnce,
robot.getPeriod(), -robot.getPeriod());
robot.getPeriod(), -robot.getPeriod() + 1e-6);

// Step forward far enough that the onInited callbacks are run. This needs to happen on
// a separate thread because they won't be run until startCompetition() has been called
// and startCompetition() will block until timing advances.
runOnExecutorThread(() -> {
LOG.log(Level.INFO, "Calling stepTiming()");
SimHooks.stepTiming(2e-6);
LOG.log(Level.INFO, "Called stepTiming()");
});

this.endNotifier = endNotifier;
SimHooks.resumeTiming();
isRobotCodeRunning = true;
try {
robot.startCompetition();
} finally {
// Always call endCompetition() so that WPILib's notifier is stopped. If it isn't
// future tests will hang on calls to stepTiming().
endCompetition(robot);
blockWhileShuttingDownExecutor();
}
if (runExitThrowable != null) {
throw new RuntimeException(runExitThrowable);
Expand All @@ -850,28 +867,43 @@ public void run() throws InstantiationException, IllegalAccessException,
}
}

private void blockWhileShuttingDownExecutor() {
if (listenerCallbackExecutor.isShutdown()) {
return;
}
try {
listenerCallbackExecutor.shutdown();
if (!listenerCallbackExecutor.awaitTermination(10,
TimeUnit.SECONDS)) {
throw new TimeoutException(
"Timed out awaiting termination of callback executor");
}
// This needs to be done after shutting down the listenerCallbackExecutor
// because some callbacks create Watchers and Watcher is not thread-safe.
Watcher.closeAll();
} catch (SecurityException | InterruptedException
| TimeoutException ex) {
throw new RuntimeException("Could not shutdown callback executor.",
ex);
}
}

private Notifier endNotifier = null;

/**
* Closes this instance, freeing any resources that in holds.
*/
public void close() {
LOG.log(Level.DEBUG, "Closing WebotsSimulator");
blockWhileShuttingDownExecutor();
requestPublisher.close();
statusSubscriber.close();
Watcher.closeAll();
inst.close();
inst.stopServer();
pauser.close();
if (timeSyncDevice != null) {
timeSyncDevice.close();
}
try {
listenerCallbackExecutor.shutdown();
} catch (SecurityException ex) {
LOG.log(Level.ERROR, "Could not shutdown callback executor.", ex);
}

LOG.log(Level.DEBUG, "Done closing WebotsSimulator");
}
}
72 changes: 42 additions & 30 deletions tests/src/systemTest/java/frc/robot/MotorControllerTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.Timer;

@Timeout(value = 30, unit = TimeUnit.MINUTES)
@ResourceLock("WebotsSimulator")
Expand Down Expand Up @@ -91,6 +92,12 @@ public class MotorControllerTest {
// Rearranging gives:
// theta(t) = theta_0 + w_f*t + (w_0 - w_f) * t_c * (1-exp(-t/t_c))


// The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e.
// sets the speed to 0 and lets it stop on it's own).
private static double tMotorStartsSecs = 0.0;
private static double tMotorStopsSecs = timeOfNextStepSecs(2.0);

private static double computeSpeedRadPerSec(DCMotor gearMotor,
double moiKgM2, double throttle, double initialSpeedRadPerSec,
double tSecs) {
Expand All @@ -104,7 +111,7 @@ private static double computeSpeedRadPerSec(DCMotor gearMotor,
}

private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2,
double throttle, double initialSpeedRadPerSec,
double throttle, double initialSpeedRadPerSec,
double initialAngleRad, double tSecs) {
double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2);
double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts
Expand All @@ -124,7 +131,7 @@ private static double computeTimeConstantSecs(DCMotor gearMotor,
}

private static double computeCylinderMoiKgM2(double radiusMeters,
double heightMeters, double densityKgPerM3) {
double heightMeters, double densityKgPerM3) {
double massKg = densityKgPerM3 * Math.PI * radiusMeters * radiusMeters
* heightMeters;
return massKg * radiusMeters * radiusMeters / 2.0;
Expand All @@ -136,12 +143,13 @@ private static double timeOfNextStepSecs(double tSecs) {

private static double expectedSpeedRadPerSec(DCMotor gearMotor,
double moiKgM2, double tSecs) {
// The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e.
// sets the speed to 0 and lets it stop on it's own).
double tMotorStopsSecs = timeOfNextStepSecs(2.0);

if (tSecs <= tMotorStartsSecs) {
return 0.0;
}
if (tSecs <= tMotorStopsSecs) {
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0, tSecs);
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0,
tSecs - tMotorStartsSecs);
}
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0,
expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs),
Expand All @@ -150,11 +158,11 @@ private static double expectedSpeedRadPerSec(DCMotor gearMotor,

private static double expectedAngleRadians(DCMotor gearMotor,
double moiKgM2, double tSecs) {
// The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e.
// sets the speed to 0 and lets it stop on it's own).
double tMotorStopsSecs = timeOfNextStepSecs(2.0);
if (tSecs <= tMotorStopsSecs) {
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, tSecs);
if (tSecs <= tMotorStartsSecs) {
return 0.0;
} else if (tSecs <= tMotorStopsSecs) {
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0,
tSecs - tMotorStartsSecs);
}
return computeAngleRadians(gearMotor, moiKgM2, 0,
expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs),
Expand All @@ -168,7 +176,7 @@ private static double computeGearing(DCMotor motor,
}

private static double computeFlywheelThickness(DCMotor gearMotor,
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
double desiredTimeConstantSecs) {
// for a time constant of t_c seconds:
// t_c = R*J*k_v/k_T
Expand All @@ -193,7 +201,8 @@ private static class Model {
double moiKgM2;
Measurement m1, m2;

Model(String motorModelName, String shaftDefPath) throws Exception {
Model(String motorModelName, String shaftDefPath, double gearing,
double flywheelThicknessMeters) throws Exception {
this.motorModelName = motorModelName;
this.shaftDefPath = shaftDefPath;
// To ensure we the flywheel doesn't spin or accelerate too fast...
Expand All @@ -207,20 +216,20 @@ private static class Model {
var motor = (DCMotor) (DCMotor.class
.getDeclaredMethod("get" + motorModelName, int.class)
.invoke(null, 1));
var gearing = computeGearing(motor, desiredFlywheelFreeSpeedRPS);
assertEquals(computeGearing(motor, desiredFlywheelFreeSpeedRPS),
gearing, 0.01 * gearing,
"Incorrect gearing for a free speed of %g RPS when using a %s"
.formatted(desiredFlywheelFreeSpeedRPS,
motorModelName));
gearMotor = motor.withReduction(gearing);
LOG.log(Level.INFO,
"For a free speed of {0} RPS when using a {1}, assuming the gearing is {2}.\n",
desiredFlywheelFreeSpeedRPS, motorModelName, gearing);
var flywheelThicknessMeters =
assertEquals(
computeFlywheelThickness(gearMotor, flywheelRadiusMeters,
flywheelDensityKgPerM3, desiredTimeConstantSecs);
LOG.log(Level.INFO,
"For a time constant of {0} second when using a {1} with a gearing of {2} and a flywheel with radius {3} meters and density {4} kg/m^3, assuming the flywheel thickness is {5} meters.\n",
desiredTimeConstantSecs, motorModelName, gearing,
flywheelRadiusMeters, flywheelDensityKgPerM3,
flywheelThicknessMeters);

flywheelDensityKgPerM3, desiredTimeConstantSecs),
flywheelThicknessMeters, 0.01 * flywheelThicknessMeters,
"Incorrect flywheel thickness for a time constant of %g second when using a %s with a gearing of %g and a flywheel with radius %g meters and density %g kg/m^3"
.formatted(desiredTimeConstantSecs, motorModelName,
gearing, flywheelRadiusMeters,
flywheelDensityKgPerM3));
// With those settings, the flywheel MOI will be:
moiKgM2 = computeCylinderMoiKgM2(flywheelRadiusMeters,
flywheelThicknessMeters, flywheelDensityKgPerM3);
Expand All @@ -233,9 +242,7 @@ public String toString() {

private void assertShaftCorrectAtSecs(double actualSpeedRadPerSec,
double actualAngleRadians, double tSecs) {
// TODO: Make sim accurate enough that this can be less than 1*simStepSizeSecs
// (https://github.com/DeepBlueRobotics/DeepBlueSim/issues/101)
double jitterSecs = 2.5 * simStepSizeSecs;
double jitterSecs = 0.5 * simStepSizeSecs;
double expectedEarlierSpeedRadPerSec = expectedSpeedRadPerSec(
gearMotor, moiKgM2, tSecs - jitterSecs);
double expectedLaterSpeedRadPerSec = expectedSpeedRadPerSec(
Expand Down Expand Up @@ -300,15 +307,20 @@ private Consumer<SimulationState> stateChecker(
};
}

// @RepeatedTest(value = 20, failureThreshold = 1)
@RepeatedTest(1)
void testCorrectRotationInAutonomous() throws Exception {
models.add(new Model("NEO", "CAN_SHAFT"));
models.add(new Model("MiniCIM", "PWM_SHAFT"));
models.add(new Model("NEO", "CAN_SHAFT", 94.6, 0.392));
models.add(new Model("MiniCIM", "PWM_SHAFT", 97.3333, 0.215));

try (var manager = new WebotsSimulator(
"../plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt",
MotorControllerRobot::new)) {
manager.atSec(0.0, s -> {
// Ensure that the timer didn't step beyond the "microstep" needed to ensure that
// the onInited callbacks get run.
assertEquals(2e-6, Timer.getFPGATimestamp(),
"Timer.getFPGATimestamp() should be 2e-6");
s.enableAutonomous();
}).setMaxJitterSecs(0).everyStep(stateChecker((s, m) -> {
LOG.log(Level.DEBUG,
Expand Down

0 comments on commit fca5060

Please sign in to comment.