diff --git a/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java b/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java index badcf9b..eb88ffc 100644 --- a/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java +++ b/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java @@ -219,7 +219,8 @@ public void run() { LOG.log(Level.DEBUG, "Loading world {0}", reloadRequest); robot.worldLoad(reloadRequest); - stepSimulation(robot, basicTimeStep); + // Allow Webots to process the request. + robot.step(0); LOG.log(Level.DEBUG, "Loaded world {0}", reloadRequest); }); }); diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java index 053899e..4120da5 100644 --- a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java +++ b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java @@ -445,8 +445,6 @@ private void startTimeSync() { return; } simTimeSec = eventValue; - // Start the robot timing if we haven't already - ensureRobotTimingStarted(); // Do nothing if the robot program is not rurnning if (!isRobotCodeRunning) { @@ -824,6 +822,11 @@ public void 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"); }