Load world via "load worldFilePath" request.

DeepBlueRobotics · Jul 17, 2024 · c0ebd9a · c0ebd9a
1 parent f86633b
commit c0ebd9a
Show file tree

Hide file tree

Showing 3 changed files with 60 additions and 33 deletions.
diff --git a/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java b/plugin/controller/src/main/java/org/carlmontrobotics/deepbluesim/WebotsSupervisor.java
@@ -213,37 +213,25 @@ public void run() {
                             request);
                     if (request == null)
                         return;
-                    var file = new File(request);
-                    if (!file.isFile()) {
-                        LOG.log(Level.ERROR,
-                                "ERROR: Received a request to load file that does not exist: {0}",
-                                request);
-                        return;
+                    var requestParts = request.split(" ", 2);
+                            var requestVerb = requestParts[0];
+                    switch (requestVerb) {
+                        case NTConstants.REQUEST_LOAD_VERB:
+                            if (requestParts.length != 2) {
+                                LOG.log(Level.ERROR,
+                                        "Ignoring malformed load request: {0}",
+                                        request);
+                            } else {
+                                handleLoadRequest(robot, basicTimeStep,
+                                        requestParts[1]);
+                            }
+                            break;
+
+                        default:
+                            LOG.log(Level.ERROR,
+                                    "Ignoring unrecognized request verb {0}",
+                                    requestVerb);
                     }
-                    queuedEvents.add(() -> {
-                        LOG.log(Level.DEBUG, "Handling request");
-                        // Ensure we don't leave any watchers waiting for values they requested
-                        // before requesting a new world.
-                        closePublishers();
-                        waitUntilFlushed();
-                        close();
-                        inst.close();
-                        delayer.cancel();
-
-                        LOG.log(Level.DEBUG,
-                                "Updating simulation speed and mode");
-                        // Unpause before loading so that the new controller can take it's
-                        // first step.
-                        updateUsersSimulationSpeed(robot);
-                        robot.simulationSetMode(usersSimulationSpeed);
-                        isWorldLoading = true;
-                        LOG.log(Level.DEBUG, "Loading world {0}",
-                                request);
-                        robot.worldLoad(request);
-                        // Allow Webots to process the request.
-                        robot.step(basicTimeStep);
-                        LOG.log(Level.DEBUG, "Loaded world {0}", request);
-                    });
                 });
 
         // Add a listener to handle simMode requests.
@@ -357,6 +345,39 @@ public void run() {
         });
     }
 
+    private static void handleLoadRequest(Supervisor robot, int basicTimeStep,
+            String worldFilePath) {
+        var file = new File(worldFilePath);
+        if (!file.isFile()) {
+            LOG.log(Level.ERROR,
+                    "ERROR: Received a request to load file that does not exist: {0}",
+                    worldFilePath);
+            return;
+        }
+        queuedEvents.add(() -> {
+            LOG.log(Level.DEBUG, "Handling request");
+            // Ensure we don't leave any watchers waiting for values they requested
+            // before requesting a new world.
+            closePublishers();
+            waitUntilFlushed();
+            close();
+            inst.close();
+            delayer.cancel();
+
+            LOG.log(Level.DEBUG, "Updating simulation speed and mode");
+            // Unpause before loading so that the new controller can take it's
+            // first step.
+            updateUsersSimulationSpeed(robot);
+            robot.simulationSetMode(usersSimulationSpeed);
+            isWorldLoading = true;
+            LOG.log(Level.DEBUG, "Loading world {0}", worldFilePath);
+            robot.worldLoad(worldFilePath);
+            // Allow Webots to process the request.
+            robot.step(basicTimeStep);
+            LOG.log(Level.DEBUG, "Loaded world {0}", worldFilePath);
+        });
+    }
+
     private static final long minQuietTimeMs = 500;
 
     private static void sendCompletedOnceHALSimIsQuiet() {

diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java
@@ -333,12 +333,15 @@ private WebotsSimulator waitForUserToStart(String worldFileAbsPath)
                                 NTConstants.STATUS_COMPLETED_VALUE))
                             return;
                         if (loadCount++ == 0) {
-                                    LOG.log(Level.DEBUG, "Sending request = {0}",
+                            var request = String.format("%s %s",
+                                    NTConstants.REQUEST_LOAD_VERB,
                                     worldFileAbsPath);
-                            requestPublisher.set(worldFileAbsPath);
+                            LOG.log(Level.DEBUG, "Sending request = {0}",
+                                            request);
+                            requestPublisher.set(request);
                             inst.flush();
                             LOG.log(Level.DEBUG, "Sent request = {0}",
-                                            worldFileAbsPath);
+                                    request);
                         } else {
                             isReady = true;
                             isReadyFuture.complete(true);

diff --git a/...ibdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/internal/NTConstants.java b/...ibdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/internal/NTConstants.java
@@ -36,6 +36,9 @@ public class NTConstants {
         /** The name of the topic that is used to tell Webots which world file to load. */
         public static final String REQUEST_TOPIC_NAME = "request";
 
+        /** The verb used to request that a world be loaded. */
+        public static final String REQUEST_LOAD_VERB = "load";
+
         /** The name of the table that contains the above topics. */
         public static final String COORDINATOR_TABLE_NAME =
                         "/DeepBlueSim/Coordinator";