Skip to content

Commit

Permalink
cleanup and minor fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@CoolSpy3
CoolSpy3 committed May 15, 2024
1 parent 64d45a9 commit 4f5f4a3
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 54 deletions.
80 changes: 37 additions & 43 deletions example/src/systemTest/java/frc/robot/SystemTestRobot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,8 @@
import java.util.concurrent.TimeoutException;

import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.HALValue;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.simulation.SimValueCallback;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
Expand All @@ -24,7 +22,11 @@

public class SystemTestRobot extends Robot {

/**
* Time value set by the simulator and the robot to indicate that the simulation should start.
*/
private static final double START_SIMULATION = -2.0;
private static final Notifier pauser = new Notifier(() -> { /* This is replaced in webotsInit */ });

public static void main(String... args) {
RobotBase.startRobot(SystemTestRobot::new);
Expand Down Expand Up @@ -60,7 +62,7 @@ public void simulationInit() {
positionZ = webotsSupervisor.createDouble("self.position.z", SimDevice.Direction.kInput, 0.0);
webotsInit();

System.out.println("Webots. Enabling in autonomous."); System.out.flush();
System.out.println("Webots has started. Enabling in autonomous."); System.out.flush();
// Simulate starting autonomous
DriverStationSim.setAutonomous(true);
DriverStationSim.setEnabled(true);
Expand All @@ -76,17 +78,12 @@ private void webotsInit() {
// startup time.
robotTime.stop();
robotTime.reset();
addPeriodic(() -> {
robotTime.start();
}, getPeriod(), -getPeriod());
addPeriodic(robotTime::start, getPeriod(), -getPeriod());
SimDevice timeSynchronizer = SimDevice.create("TimeSynchronizer");
SimDouble simTimeSecSim = timeSynchronizer.createDouble("simTimeSec", SimDevice.Direction.kInput, -1.0);
final SimDouble robotTimeSecSim = timeSynchronizer.createDouble("robotTimeSec", SimDevice.Direction.kOutput, -1.0);
SimDeviceSim timeSynchronizerSim = new SimDeviceSim("TimeSynchronizer");

final Notifier pauser = new Notifier(() -> {
// This is replaced on the next line
});
pauser.setHandler(() -> {
double simTimeSec = simTimeSecSim.get();
double robotTimeSec = robotTime.get();
Expand All @@ -105,45 +102,42 @@ private void webotsInit() {

final var isReadyFuture = new CompletableFuture<Boolean>();

timeSynchronizerSim.registerValueChangedCallback(simTimeSecSim, new SimValueCallback() {
@Override
public synchronized void callback(String name, int handle, int direction, HALValue value) {
double simTimeSec = value.getDouble();
double robotTimeSec = robotTime.get();
timeSynchronizerSim.registerValueChangedCallback(simTimeSecSim, (name, handle, direction, value) -> {
double simTimeSec = value.getDouble();
double robotTimeSec = robotTime.get();

// Ignore the default initial value
if (simTimeSec == -1.0) {
return;
}
// If we asked for the simulation to start and it has started, say that we're ready.
if (robotTimeSecSim.get() == START_SIMULATION) {
if (simTimeSec == START_SIMULATION) {
isReadyFuture.complete(true);
robotTimeSecSim.set(robotTimeSec);
}
return;
}
// Otherwise, ignore notifications that the sim has started.
// Ignore the default initial value
if (simTimeSec == -1.0) {
return;
}
// If we asked for the simulation to start and it has started, say that we're ready.
if (robotTimeSecSim.get() == START_SIMULATION) {
if (simTimeSec == START_SIMULATION) {
return;
}

// If we're not behind the sim time, there is nothing to do.
double deltaSecs = simTimeSec - robotTimeSec;
if (deltaSecs < 0.0) {
return;
isReadyFuture.complete(true);
robotTimeSecSim.set(robotTimeSec);
}
return;
}
// Otherwise, ignore notifications that the sim has started.
if (simTimeSec == START_SIMULATION) {
return;
}

// We are behind the sim time, so run until we've caught up.
// We use a Notifier instead of SimHooks.stepTiming() because
// using SimHooks.stepTiming() causes accesses to sim data to block.
pauser.stop();
pauser.startSingle(deltaSecs);
SimHooks.resumeTiming();
// If we're not behind the sim time, there is nothing to do.
double deltaSecs = simTimeSec - robotTimeSec;
if (deltaSecs <= 0.0) {
return;
}

// We are behind the sim time, so run until we've caught up.
// We use a Notifier instead of SimHooks.stepTiming() because
// using SimHooks.stepTiming() causes accesses to sim data to block.
pauser.stop();
pauser.startSingle(deltaSecs);
SimHooks.resumeTiming();
}, true);

// Reset the clock. Without this, *Periodic calls that should have
// Reset the clock. Without this, *Periodic calls that should have
// occurred while we waited, will be considered behind schedule and
// will all happen at once.
SimHooks.restartTiming();
Expand All @@ -153,7 +147,7 @@ public synchronized void callback(String name, int handle, int direction, HALVal

// Tell sim to start
robotTimeSecSim.set(START_SIMULATION);

// Wait up to 15 minutes for Webots to respond. On GitHub's MacOS Continuous
// Integration servers, it can take over 8 minutes for Webots to start.
var startedWaitingTimeMs = System.currentTimeMillis();
Expand All @@ -166,7 +160,7 @@ public synchronized void callback(String name, int handle, int direction, HALVal
if(remainingTime > 0) {
isReady = isReadyFuture.get(remainingTime, TimeUnit.MILLISECONDS);
}
else isReady = true;
else break;
} catch (TimeoutException ex) {
System.err.println("Waiting for Webots to be ready. Please open example/Webots/worlds/DBSExample.wbt in Webots.");
} catch (InterruptedException|ExecutionException e) {
Expand Down
26 changes: 15 additions & 11 deletions plugin/controller/src/main/java/DeepBlueSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,14 @@ public class DeepBlueSim {
private static ScopedObject<Pair<String, StringCallback>> robotTimeSecCallbackStore = null;
private static RunningObject<WebSocketClient> wsConnection = null;

/**
* Time value set by the simulator and the robot to indicate that the simulation should start.
*/
private static final double START_SIMULATION = -2.0;

/**
* The time in milliseconds since the last timestep update from the robot.
*/
private static volatile long lastStepMillis = 0;

public static void main(String[] args) {
Expand Down Expand Up @@ -72,14 +78,12 @@ public void uncaughtException(Thread arg0, Throwable arg1) {
final SimDeviceSim timeSynchronizerSim = new SimDeviceSim("TimeSynchronizer");

// Regularly report the simulated robot's position
Simulation.registerPeriodicMethod(new Runnable() {
public void run() {
Node self = robot.getSelf();
double[] pos = self.getPosition();
webotsSupervisorSim.set("self.position.x", pos[0]);
webotsSupervisorSim.set("self.position.y", pos[1]);
webotsSupervisorSim.set("self.position.z", pos[2]);
}
Simulation.registerPeriodicMethod(() -> {
Node self = robot.getSelf();
double[] pos = self.getPosition();
webotsSupervisorSim.set("self.position.x", pos[0]);
webotsSupervisorSim.set("self.position.y", pos[1]);
webotsSupervisorSim.set("self.position.z", pos[2]);
});

// Pause the simulator if it hasn't taken any steps in the last 1-2 seconds
Expand All @@ -101,7 +105,7 @@ public void run() {
@Override
public synchronized void callback(String name, String value) {
// Ignore null default initial value
if (value == null)
if (value == null)
return;

double robotTimeSec = Double.parseDouble(value);
Expand All @@ -117,7 +121,7 @@ public synchronized void callback(String name, String value) {

// Keep stepping the simulation forward until the sim time is more than the robot time
// or the simulation ends.
while(true) {
for(;;) {
double simTimeSec = robot.getTime();
if (simTimeSec > robotTimeSec) {
break;
Expand Down Expand Up @@ -175,7 +179,7 @@ public synchronized void callback(String name, String value) {
queuedMessages.takeFirst().run();
}
} catch (Exception ex) {
throw new RuntimeException("Exception while waiting for simulation to be done");
throw new RuntimeException("Exception while waiting for simulation to be done", ex);
}

System.out.println("Shutting down DeepBlueSim...");
Expand Down

0 comments on commit 4f5f4a3

Please sign in to comment.