Skip to content

Commit fca5060

Browse files
brettlebrettle
authored
Merge pull request #124 from DeepBlueRobotics/fix-tests-not-repeatable
Make tests repeatable and more accurate.
2 parents 5be9c56 + d4079a0 commit fca5060

File tree

2 files changed

+98
-54
lines changed

2 files changed

+98
-54
lines changed

‎plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java

Lines changed: 56 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -86,8 +86,7 @@ public class WebotsSimulator implements AutoCloseable {
8686
private final StringPublisher simModePublisher;
8787

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

9291
// Use these to control NetworkTables logging.
9392
// - ntLoglevel = 0 means no NT logging
@@ -142,6 +141,13 @@ public WebotsSimulator setMaxJitterSecs(double maxJitterSecs) {
142141
}
143142
}
144143

144+
private void runOnExecutorThread(Runnable r) {
145+
if (listenerCallbackExecutor.isShutdown()) {
146+
return;
147+
}
148+
listenerCallbackExecutor.execute(r);
149+
}
150+
145151
private static synchronized void acquireFileLock()
146152
throws InterruptedException, IOException {
147153
if (fileLock == null) {
@@ -326,7 +332,7 @@ private WebotsSimulator waitForUserToStart(String worldFileAbsPath)
326332
inst.addListener(statusSubscriber,
327333
EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> {
328334
final var eventValue = event.valueData.value.getString();
329-
listenerCallbackExecutor.execute(() -> {
335+
runOnExecutorThread(() -> {
330336
LOG.log(Level.DEBUG, "In listener, status = {0}",
331337
eventValue);
332338
if (!eventValue.equals(
@@ -433,7 +439,7 @@ private void startTimeSync() {
433439
LOG.log(Level.DEBUG,
434440
"In simTimeSec value changed callback");
435441
final var eventValue = value.getDouble();
436-
listenerCallbackExecutor.execute(() -> {
442+
runOnExecutorThread(() -> {
437443
LOG.log(Level.DEBUG, "Got simTimeSec value of {0}",
438444
eventValue);
439445
if (eventValue < 0.0) {
@@ -491,7 +497,7 @@ private void startTimeSync() {
491497

492498
waitForHALSimWSConnection();
493499

494-
listenerCallbackExecutor.execute(() -> {
500+
runOnExecutorThread(() -> {
495501
simTimeSec = 0.0;
496502
sendRobotTime();
497503
});
@@ -506,14 +512,12 @@ private void waitForHALSimWSConnection() {
506512
inst.addListener(statusSubscriber,
507513
EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> {
508514
final var eventValue = event.valueData.value.getString();
509-
listenerCallbackExecutor.execute(() -> {
510-
LOG.log(Level.DEBUG, "In listener, status = {0}",
511-
eventValue);
512-
if (!eventValue.equals(
513-
NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE))
514-
return;
515-
halSimWSConnected.complete(null);
516-
});
515+
LOG.log(Level.DEBUG, "In listener, status = {0}",
516+
eventValue);
517+
if (!eventValue.equals(
518+
NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE))
519+
return;
520+
halSimWSConnected.complete(null);
517521
});
518522

519523
requestPublisher.set(NTConstants.REQUEST_HALSIMWS_CONNECTION_VERB);
@@ -790,6 +794,8 @@ private void endCompetition(TimedRobot robot) {
790794
public void run() throws InstantiationException, IllegalAccessException,
791795
InvocationTargetException, NoSuchMethodException,
792796
SecurityException, TimeoutException {
797+
listenerCallbackExecutor = Executors.newSingleThreadExecutor();
798+
793799
endCompetitionCalled = false;
794800
// HAL must be initialized or SmartDashboard might not work.
795801
HAL.initialize(500, 0);
@@ -821,20 +827,31 @@ public void run() throws InstantiationException, IllegalAccessException,
821827
});
822828
}
823829

824-
// Run the onInited callbacks once.
825-
// Note: the offset is -period so they are run before other WPILib periodic methods
830+
// Schedule the onInited callbacks to be run once. Note: the offset is -period so they
831+
// are run before other WPILib periodic methods the 1e-6 is so that it isn't scheduled
832+
// for a timestamp of exactly 0.0 because that is a special value that causes
833+
// startCompetition() to end.
826834
robot.addPeriodic(this::runRobotInitedCallbacksOnce,
827-
robot.getPeriod(), -robot.getPeriod());
835+
robot.getPeriod(), -robot.getPeriod() + 1e-6);
836+
837+
// Step forward far enough that the onInited callbacks are run. This needs to happen on
838+
// a separate thread because they won't be run until startCompetition() has been called
839+
// and startCompetition() will block until timing advances.
840+
runOnExecutorThread(() -> {
841+
LOG.log(Level.INFO, "Calling stepTiming()");
842+
SimHooks.stepTiming(2e-6);
843+
LOG.log(Level.INFO, "Called stepTiming()");
844+
});
828845

829846
this.endNotifier = endNotifier;
830-
SimHooks.resumeTiming();
831847
isRobotCodeRunning = true;
832848
try {
833849
robot.startCompetition();
834850
} finally {
835851
// Always call endCompetition() so that WPILib's notifier is stopped. If it isn't
836852
// future tests will hang on calls to stepTiming().
837853
endCompetition(robot);
854+
blockWhileShuttingDownExecutor();
838855
}
839856
if (runExitThrowable != null) {
840857
throw new RuntimeException(runExitThrowable);
@@ -850,28 +867,43 @@ public void run() throws InstantiationException, IllegalAccessException,
850867
}
851868
}
852869

870+
private void blockWhileShuttingDownExecutor() {
871+
if (listenerCallbackExecutor.isShutdown()) {
872+
return;
873+
}
874+
try {
875+
listenerCallbackExecutor.shutdown();
876+
if (!listenerCallbackExecutor.awaitTermination(10,
877+
TimeUnit.SECONDS)) {
878+
throw new TimeoutException(
879+
"Timed out awaiting termination of callback executor");
880+
}
881+
// This needs to be done after shutting down the listenerCallbackExecutor
882+
// because some callbacks create Watchers and Watcher is not thread-safe.
883+
Watcher.closeAll();
884+
} catch (SecurityException | InterruptedException
885+
| TimeoutException ex) {
886+
throw new RuntimeException("Could not shutdown callback executor.",
887+
ex);
888+
}
889+
}
890+
853891
private Notifier endNotifier = null;
854892

855893
/**
856894
* Closes this instance, freeing any resources that in holds.
857895
*/
858896
public void close() {
859897
LOG.log(Level.DEBUG, "Closing WebotsSimulator");
898+
blockWhileShuttingDownExecutor();
860899
requestPublisher.close();
861900
statusSubscriber.close();
862-
Watcher.closeAll();
863901
inst.close();
864902
inst.stopServer();
865903
pauser.close();
866904
if (timeSyncDevice != null) {
867905
timeSyncDevice.close();
868906
}
869-
try {
870-
listenerCallbackExecutor.shutdown();
871-
} catch (SecurityException ex) {
872-
LOG.log(Level.ERROR, "Could not shutdown callback executor.", ex);
873-
}
874-
875907
LOG.log(Level.DEBUG, "Done closing WebotsSimulator");
876908
}
877909
}

‎tests/src/systemTest/java/frc/robot/MotorControllerTest.java

Lines changed: 42 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919

2020
import edu.wpi.first.math.MathUtil;
2121
import edu.wpi.first.math.system.plant.DCMotor;
22+
import edu.wpi.first.wpilibj.Timer;
2223

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

95+
96+
// The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e.
97+
// sets the speed to 0 and lets it stop on it's own).
98+
private static double tMotorStartsSecs = 0.0;
99+
private static double tMotorStopsSecs = timeOfNextStepSecs(2.0);
100+
94101
private static double computeSpeedRadPerSec(DCMotor gearMotor,
95102
double moiKgM2, double throttle, double initialSpeedRadPerSec,
96103
double tSecs) {
@@ -104,7 +111,7 @@ private static double computeSpeedRadPerSec(DCMotor gearMotor,
104111
}
105112

106113
private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2,
107-
double throttle, double initialSpeedRadPerSec,
114+
double throttle, double initialSpeedRadPerSec,
108115
double initialAngleRad, double tSecs) {
109116
double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2);
110117
double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts
@@ -124,7 +131,7 @@ private static double computeTimeConstantSecs(DCMotor gearMotor,
124131
}
125132

126133
private static double computeCylinderMoiKgM2(double radiusMeters,
127-
double heightMeters, double densityKgPerM3) {
134+
double heightMeters, double densityKgPerM3) {
128135
double massKg = densityKgPerM3 * Math.PI * radiusMeters * radiusMeters
129136
* heightMeters;
130137
return massKg * radiusMeters * radiusMeters / 2.0;
@@ -136,12 +143,13 @@ private static double timeOfNextStepSecs(double tSecs) {
136143

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

147+
if (tSecs <= tMotorStartsSecs) {
148+
return 0.0;
149+
}
143150
if (tSecs <= tMotorStopsSecs) {
144-
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0, tSecs);
151+
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0,
152+
tSecs - tMotorStartsSecs);
145153
}
146154
return computeSpeedRadPerSec(gearMotor, moiKgM2, 0,
147155
expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs),
@@ -150,11 +158,11 @@ private static double expectedSpeedRadPerSec(DCMotor gearMotor,
150158

151159
private static double expectedAngleRadians(DCMotor gearMotor,
152160
double moiKgM2, double tSecs) {
153-
// The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e.
154-
// sets the speed to 0 and lets it stop on it's own).
155-
double tMotorStopsSecs = timeOfNextStepSecs(2.0);
156-
if (tSecs <= tMotorStopsSecs) {
157-
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, tSecs);
161+
if (tSecs <= tMotorStartsSecs) {
162+
return 0.0;
163+
} else if (tSecs <= tMotorStopsSecs) {
164+
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0,
165+
tSecs - tMotorStartsSecs);
158166
}
159167
return computeAngleRadians(gearMotor, moiKgM2, 0,
160168
expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs),
@@ -168,7 +176,7 @@ private static double computeGearing(DCMotor motor,
168176
}
169177

170178
private static double computeFlywheelThickness(DCMotor gearMotor,
171-
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
179+
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
172180
double desiredTimeConstantSecs) {
173181
// for a time constant of t_c seconds:
174182
// t_c = R*J*k_v/k_T
@@ -193,7 +201,8 @@ private static class Model {
193201
double moiKgM2;
194202
Measurement m1, m2;
195203

196-
Model(String motorModelName, String shaftDefPath) throws Exception {
204+
Model(String motorModelName, String shaftDefPath, double gearing,
205+
double flywheelThicknessMeters) throws Exception {
197206
this.motorModelName = motorModelName;
198207
this.shaftDefPath = shaftDefPath;
199208
// To ensure we the flywheel doesn't spin or accelerate too fast...
@@ -207,20 +216,20 @@ private static class Model {
207216
var motor = (DCMotor) (DCMotor.class
208217
.getDeclaredMethod("get" + motorModelName, int.class)
209218
.invoke(null, 1));
210-
var gearing = computeGearing(motor, desiredFlywheelFreeSpeedRPS);
219+
assertEquals(computeGearing(motor, desiredFlywheelFreeSpeedRPS),
220+
gearing, 0.01 * gearing,
221+
"Incorrect gearing for a free speed of %g RPS when using a %s"
222+
.formatted(desiredFlywheelFreeSpeedRPS,
223+
motorModelName));
211224
gearMotor = motor.withReduction(gearing);
212-
LOG.log(Level.INFO,
213-
"For a free speed of {0} RPS when using a {1}, assuming the gearing is {2}.\n",
214-
desiredFlywheelFreeSpeedRPS, motorModelName, gearing);
215-
var flywheelThicknessMeters =
225+
assertEquals(
216226
computeFlywheelThickness(gearMotor, flywheelRadiusMeters,
217-
flywheelDensityKgPerM3, desiredTimeConstantSecs);
218-
LOG.log(Level.INFO,
219-
"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",
220-
desiredTimeConstantSecs, motorModelName, gearing,
221-
flywheelRadiusMeters, flywheelDensityKgPerM3,
222-
flywheelThicknessMeters);
223-
227+
flywheelDensityKgPerM3, desiredTimeConstantSecs),
228+
flywheelThicknessMeters, 0.01 * flywheelThicknessMeters,
229+
"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"
230+
.formatted(desiredTimeConstantSecs, motorModelName,
231+
gearing, flywheelRadiusMeters,
232+
flywheelDensityKgPerM3));
224233
// With those settings, the flywheel MOI will be:
225234
moiKgM2 = computeCylinderMoiKgM2(flywheelRadiusMeters,
226235
flywheelThicknessMeters, flywheelDensityKgPerM3);
@@ -233,9 +242,7 @@ public String toString() {
233242

234243
private void assertShaftCorrectAtSecs(double actualSpeedRadPerSec,
235244
double actualAngleRadians, double tSecs) {
236-
// TODO: Make sim accurate enough that this can be less than 1*simStepSizeSecs
237-
// (https://github.com/DeepBlueRobotics/DeepBlueSim/issues/101)
238-
double jitterSecs = 2.5 * simStepSizeSecs;
245+
double jitterSecs = 0.5 * simStepSizeSecs;
239246
double expectedEarlierSpeedRadPerSec = expectedSpeedRadPerSec(
240247
gearMotor, moiKgM2, tSecs - jitterSecs);
241248
double expectedLaterSpeedRadPerSec = expectedSpeedRadPerSec(
@@ -300,15 +307,20 @@ private Consumer<SimulationState> stateChecker(
300307
};
301308
}
302309

310+
// @RepeatedTest(value = 20, failureThreshold = 1)
303311
@RepeatedTest(1)
304312
void testCorrectRotationInAutonomous() throws Exception {
305-
models.add(new Model("NEO", "CAN_SHAFT"));
306-
models.add(new Model("MiniCIM", "PWM_SHAFT"));
313+
models.add(new Model("NEO", "CAN_SHAFT", 94.6, 0.392));
314+
models.add(new Model("MiniCIM", "PWM_SHAFT", 97.3333, 0.215));
307315

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

0 commit comments

Comments
 (0)