first successful mini-run

ros3rag.common/src/main/resources/jastadd/types.jadd

@@ -188,6 +188,10 @@ aspect Navigation {
   }
   syn List<String> Region.locationNamesAsList() = arrayAsList(getLocationNames()) ;
   syn List<DropOffLocation> Region.locationList();
+
+  syn Set<String> LogicalMovableObject.regionNameSet() {
+    return getLocatedAtList().stream().map(LogicalRegion::getName).collect(java.util.stream.Collectors.toSet());
+  }
 }
 
 aspect Printing {

ros3rag.placeB/src/main/jastadd/WorldModelB.jadd

@@ -73,7 +73,7 @@ aspect Computation {
         continue;
       }
       if (!myGenericObject.isLogicalMovableObject()) {
-        System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!");
+        System.err.println("MovableObject " + otherObject.getName() + " is not a MovableObject in myScene. Skipping!");
         continue;
       }
       LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject();
@@ -94,8 +94,8 @@ aspect Computation {
       if (otherObject.getLocatedAtList().size() > 1) {
         System.err.println("More than one region found for other " + otherObject.getName() + ", using only first.");
       }
-      Set<LogicalRegion> myRegions = new HashSet<>(myObject.getLocatedAtList());
-      Set<LogicalRegion> otherRegions = new HashSet<>(otherObject.getLocatedAtList());
+      Set<String> myRegions = myObject.regionNameSet();
+      Set<String> otherRegions = otherObject.regionNameSet();
       if (!myObject.hasLocatedAt() && !otherObject.hasLocatedAt() ||
           (myObject.hasLocatedAt() && otherObject.hasLocatedAt() &&
               myRegions.equals(otherRegions))) {
@@ -189,7 +189,7 @@ aspect Computation {
               }
               return result;
             }).orElseGet(() -> error("Could not resolve graph vertex of new region " + getNewRegion().nameAndHash()));
-          }).orElseGet(() -> error("Could not find a free location in " + getNewRegion().nameAndHash()));
+          }).orElseGet(() -> error("Could not find a free location in " + getNewRegion().nameAndHash() + " for " + getObject().nameAndHash()));
     }).orElseGet(() -> error("Could not resolve graph vertex of previous location " + previousLocation.nameAndHash()));
   }
   eq DifferenceNewObject.computeOperations() {

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java

@@ -149,53 +149,77 @@ public class SimpleMainB {
     LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene();
     byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA);
 
-    logger.info("Wait 2 sec, then send new logical scene");
-    TimeUnit.SECONDS.sleep(2);
+    describedWait(1, "send new logical scene");
     mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend);
 
-    logger.info("Wait another 2 sec, then print model status");
-    TimeUnit.SECONDS.sleep(2);
+    describedWait(2, "print model status");
     mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
 
-    logger.info("Wait another 2 sec, then send updated sceneB");
-    TimeUnit.SECONDS.sleep(2);
-
     // set object O1 to position of P-E in sceneB and publish it
-    Position posOfPE = myScene.resolveObjectOfInterest("P-E").getPosition();
+    describedWait(3, "send updated sceneB (P-E)");
+    scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P-E").getPosition());
+    mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
+
+    describedWait(4, "set R1 to not busy");
+    model.findRobot("R1").ifPresentOrElse(robot -> robot.setBusy(false), () -> logger.warn("Did not find R1"));
+
+    describedWait(5, "print model status");
+    mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
+    // set object O1 to position of P2.2 in sceneB and publish it
+    describedWait(6, "send updated sceneB (P2.2)");
+    scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P2.2").getPosition());
+    mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
 
+    describedWait(7, "print model status");
+    mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
+    describedWait(8, "set R2 to not busy");
+    model.findRobot("R2").ifPresentOrElse(robot -> robot.setBusy(false), () -> logger.warn("Did not find R2"));
+
+    describedWait(9, "print model status");
+    mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
+    if (exitAutomatically) {
+      TimeUnit.SECONDS.sleep(5);
+      exitCondition.countDown();
+    }
+
+    exitCondition.await();
+  }
+
+  static void describedWait(int stepNr, String description) throws InterruptedException {
+    logger.info("({}) Wait 2 secs, then {}", stepNr, description);
+    TimeUnit.SECONDS.sleep(2);
+    logger.info(description);
+  }
+
+  static de.tudresden.inf.st.ceti.Scene updatePositionOfObject(
+      de.tudresden.inf.st.ceti.Scene scene,
+      String objectName,
+      Position newPosition) {
     List<Object> objectsList = scene.getObjectsList();
     Object newObj = null;
     int index, objectsListSize;
     for (index = 0, objectsListSize = objectsList.size(); index < objectsListSize; index++) {
       Object obj = objectsList.get(index);
-      if (obj.getId().equals("O1")) {
+      if (obj.getId().equals(objectName)) {
         Object.Builder builder = obj.toBuilder();
         builder.getPosBuilder()
-            .setX(posOfPE.getX())
-            .setY(posOfPE.getY())
-            .setZ(posOfPE.getZ());
+            .setX(newPosition.getX())
+            .setY(newPosition.getY())
+            .setZ(newPosition.getZ());
         newObj = builder.build();
         break;
       }
     }
     if (newObj == null) {
-      logger.error("Did not find object O1!");
+      logger.error("Did not find object {}!", objectName);
     } else {
       scene = scene.toBuilder().setObjects(index, newObj).build();
-      logger.info("New scene: {}", scene);
-      mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
-    }
-
-    logger.info("Wait another 2 sec, then print model status");
-    TimeUnit.SECONDS.sleep(2);
-    mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
-
-    if (exitAutomatically) {
-      TimeUnit.SECONDS.sleep(5);
-      exitCondition.countDown();
+      logger.info("Update {} in scene to {}", objectName, newObj);
     }
-
-    exitCondition.await();
+    return scene;
   }
 
   // copied from WorldModelA

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java

@@ -84,7 +84,7 @@ public class UtilB {
       return "\"" + objName + "\"";
     }
     try {
-      return model.getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(objName).getName();
+      return model.getMyScene().resolveObjectOfInterest(objName).getName();
     } catch (NullPointerException ignore) {
       return "+" + objName + "(not resolved)+";
     }
@@ -98,7 +98,10 @@ public class UtilB {
     StringBuilder sb = new StringBuilder();
     sb.append("myRobots: ")
         .append(model.getRobotList().prettyPrint(
-            robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper()
+            robot -> robot.getName()
+                + "(" + (robot.getBusy() ? "busy" : "free")
+                + ", canReach: "
+                + robot.getCanReachObjectOfInterestWrapper()
                 .getCanReachObjectOfInterestList()
                 .prettyPrint(canReachObj -> resolveObjName(model, canReachObj.getObjectName()))
                 + ")"))