update

- generalize demo commands
- avoid sending duplicate commands for same robot
- direct manipulation in mappings is dangerous (tested, added only for documentation)
- use NTAs for error operations

buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle

@@ -15,6 +15,5 @@ dependencies {
   relast group: 'org.jastadd', name: 'relast', version: "0.4.0-143"
   ragconnect group: 'de.tudresden.inf.st', name: 'ragconnect', version: '1.0.0-alpha-203'
 
-  implementation group: 'de.tudresden.inf.st', name: 'dumpAst', version: '1.0.2-68'
+  implementation group: 'de.tudresden.inf.st', name: 'dumpAst', version: '1.0.3-69'
 }
-

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

@@ -46,7 +46,9 @@ ConvertCommand maps de.tudresden.inf.st.ceti.Command command to Operation {:
 
 // --- sending ---
 send WorldModelB.NextOperation using PrintOperation ;
+// (direct manipulation is dangerous) send WorldModelB.NextOperation using PrintAndRememberOperation ;
 send Robot.myPosition(String) ;
+// (direct manipulation is dangerous) send Robot.myPosition(String) using ImmediateUpdate ;
 
 PrintOperation maps Operation op to byte[] {:
   byte[] result = op.toProtobufByteArray();
@@ -56,6 +58,23 @@ PrintOperation maps Operation op to byte[] {:
   return result;
 :}
 
+// (direct manipulation is dangerous)
+//PrintAndRememberOperation maps Operation op to byte[] {:
+//  byte[] result = op.toProtobufByteArray();
+//  if (result == null) {
+//    reject();
+//  }
+//  ((WorldModelB) this).addExecutedOperation(op);
+//  return result;
+//:}
+//
+// (direct manipulation is dangerous)
+//ImmediateUpdate maps String pos to String {:
+//  Robot robot = (Robot) this;
+//  robot.setCurrentPosition(pos);
+//  return pos;
+//:}
+
 ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
   de.tudresden.inf.st.ceti.Command command = null;
   try {

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

@@ -276,17 +276,37 @@ aspect Computation {
     return new Wait();
   }
 
+  syn Operation WorldModelB.lastOperationFor(Robot robot) {
+    if (robot == null) {
+      return null;
+    }
+    for (int index = getNumExecutedOperation() - 1; index >= 0; index--) {
+      Operation op = getExecutedOperation(index);
+      if (robot.equals(op.getRobotToExecute())) {
+        return op;
+      }
+    }
+    return null;
+  }
+
+  syn nta ErrorOperation WorldModelB.errorNoOperationComputed() = new ErrorOperation().setErrorMessage("No operation computed!");
+  syn nta ErrorOperation WorldModelB.errorDuplicateOperation() = new ErrorOperation().setErrorMessage("Do not send duplicate operation twice!");
+  syn nta ErrorOperation WorldModelB.errorNoExecutableOperation() = new ErrorOperation().setErrorMessage("No executable operation found!");
   //--- getNextOperation ---
   syn Operation WorldModelB.getNextOperation() {
     if (diffToOperations().getNumChild() == 0) {
-      return new ErrorOperation().setErrorMessage("No operation computed!");
+      return errorNoOperationComputed();
     }
     for (Operation op : diffToOperations()) {
+      Robot executingRobot = op.getRobotToExecute();
       if (!op.isErrorOperation()) {
+        if (op.equals(lastOperationFor(executingRobot))) {
+          return errorDuplicateOperation();
+        }
         return op;
       }
     }
-    return new ErrorOperation().setErrorMessage("No executable operation found!");
+    return errorNoExecutableOperation();
   }
 
   //--- canReach ---
@@ -427,7 +447,41 @@ aspect Navigation {
   syn boolean Operation.isErrorOperation() = false;
   eq ErrorOperation.isErrorOperation() = true;
 
-  syn Region LogicalRegion.realRegion() = worldModelB().findRegion(getName());
+  syn boolean Operation.isPickAndPlace() = false;
+  eq PickAndPlace.isPickAndPlace() = true;
+
+  syn boolean Operation.isConfigChange() = false;
+  eq ConfigChange.isConfigChange() = true;
+
+  syn boolean Operation.isEvacuate() = false;
+  eq Evacuate.isEvacuate() = true;
+
+  syn ErrorOperation Operation.asErrorOperation() = null;
+  eq ErrorOperation.asErrorOperation() = this;
+
+  syn PickAndPlace Operation.asPickAndPlace() = null;
+  eq PickAndPlace.asPickAndPlace() = this;
+
+  syn ConfigChange Operation.asConfigChange() = null;
+  eq ConfigChange.asConfigChange() = this;
+
+  syn Evacuate Operation.asEvacuate() = null;
+  eq Evacuate.asEvacuate() = this;
+
+  syn Region LogicalRegion.realRegion() {
+    if (hasWorldModelB()) {
+      return worldModelB().findRegion(getName());
+    }
+    return new Region().setName("unknown region for " + getName());
+  }
+
+  private boolean LogicalRegion.hasWorldModelB() {
+    ASTNode parent = getParent();
+    while (parent.getParent() != null) {
+      parent = parent.getParent();
+    }
+    return parent instanceof WorldModelB;
+  }
 
   //--- allMappings ---
   //  inh LocationMapping LogicalDropOffLocation.allMappings();
@@ -458,7 +512,9 @@ aspect Navigation {
   syn List<String> Robot.occupiedCollaborationZonesAsList() = arrayAsList(getOccupiedCollaborationZoneNames());
   eq Region.locationList() {
     List<DropOffLocation> result = new ArrayList<>();
-    if (!worldModelB().hasMyScene()) { return result; }
+    if (!worldModelB().hasMyScene()) {
+      return result;
+    }
     for (String locationName : locationNamesAsList()) {
       result.add(worldModelB().getMyScene().resolveObjectOfInterest(locationName).asDropOffLocation());
     }
@@ -485,7 +541,8 @@ aspect GlueForShared {
   inh LogicalObjectOfInterest Difference.resolveLogicalObjectOfInterest(String name);
   eq WorldModelB.diffScenes().resolveLogicalObjectOfInterest(String name) = getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(name);
 
-  class WorldModelB implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {}
+  class WorldModelB implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {
+  }
 
   eq WorldModelB.getMyScene().regionList() = getRegionList();
 }
@@ -543,6 +600,35 @@ aspect DumpAst {
   }
 }
 
+aspect Comparison {
+  boolean Operation.equals(Operation other) { return false; }
+  boolean PickAndPlace.equals(Operation other) {
+    if (other == null || !other.isPickAndPlace()) {
+      return false;
+    }
+    PickAndPlace otherPickAndPlace = other.asPickAndPlace();
+    return getRobotToExecute().equals(otherPickAndPlace.getRobotToExecute()) &&
+        getObjectToPick().getName().equals(otherPickAndPlace.getObjectToPick().getName()) &&
+        getTargetLocation().getName().equals(otherPickAndPlace.getTargetLocation().getName());
+  }
+  boolean ConfigChange.equals(Operation other) {
+    if (other == null || !other.isConfigChange()) {
+      return false;
+    }
+    ConfigChange otherConfigChange = other.asConfigChange();
+    return getRobotToExecute().equals(otherConfigChange.getRobotToExecute()) &&
+        getCollaborationZone().getName().equals(otherConfigChange.getCollaborationZone().getName());
+  }
+  boolean Evacuate.equals(Operation other) {
+    if (other == null || !other.isEvacuate()) {
+      return false;
+    }
+    Evacuate otherEvacuate = other.asEvacuate();
+    return getRobotToExecute().equals(otherEvacuate.getRobotToExecute()) &&
+        getCollaborationZone().getName().equals(otherEvacuate.getCollaborationZone().getName());
+  }
+}
+
 aspect Resolving {
   //--- findRobot ---
   public Optional<Robot> WorldModelB.findRobot(String name) {

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

@@ -44,13 +44,20 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
   @Override
   protected void createSpecificMainHandlerConnections() {
     mainHandler.newConnection("place-b/demo", bytes -> {
-      String topic = new String(bytes);
+      String command = new String(bytes);
+      int colonIndex = command.indexOf(":");
+      if (colonIndex == -1) {
+        logger.error("Unknown demo command {}", command);
+        return;
+      }
+      String key = command.substring(0, colonIndex);
+      String value = command.substring(colonIndex + 1);
       int slashIndex;
-      switch (topic) {
-        case "objectRed1/red":
-          UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed");
-          break;
-        case "initial_scene":
+      switch (key) {
+        case "scene":
+          if (!value.equals("initial")) {
+            logger.warn("Can only send initial scene, but got {}. Sending initial scene.", command);
+          }
           try {
             demo_scene = Util.readScene(UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-placeworld-manual.json"));
             mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray());
@@ -58,33 +65,17 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
             e.printStackTrace();
           }
           break;
-        case "arm1/idle":
-        case "arm1/picking":
-        case "arm1/placing":
-        case "arm1/moving":
-        case "arm2/idle":
-        case "arm2/picking":
-        case "arm2/placing":
-        case "arm2/moving":
-          slashIndex = topic.indexOf("/");
-          String robot = topic.substring(0, slashIndex);
-          String stateString = "STATE_" + topic.substring(slashIndex + 1).toUpperCase();
+        case "robot":
+          slashIndex = value.indexOf("/");
+          String robot = value.substring(0, slashIndex);
+          String stateString = "STATE_" + value.substring(slashIndex + 1).toUpperCase();
           Object.State state = Object.State.valueOf(stateString);
           updateAndPublishScene(robot, r -> r.toBuilder().setState(state).build());
           break;
-        case "bigBlue/cz1":
-        case "bigBlue/G1":
-        case "bigBlue/G2":
-        case "bigBlue/I1":
-        case "bigBlue/I2":
-        case "bigGreen/cz1":
-        case "bigGreen/G1":
-        case "bigGreen/G2":
-        case "bigGreen/I1":
-        case "bigGreen/I2":
-          slashIndex = topic.indexOf("/");
-          String obj = topic.substring(0, slashIndex);
-          String location = topic.substring(slashIndex + 1);
+        case "object":
+          slashIndex = value.indexOf("/");
+          String obj = value.substring(0, slashIndex);
+          String location = value.substring(slashIndex + 1);
           Position pos = model.getMyScene().resolveObjectOfInterest(location).asDropOffLocation().getPosition();
           updateAndPublishScene(obj, o -> {
             Object.Builder builder = o.toBuilder();
@@ -96,7 +87,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
           });
           break;
         default:
-          logger.error("Unknown demo command {}", topic);
+          logger.error("Unknown demo command {}", command);
       }
     });
   }
@@ -171,6 +162,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
 
   @Override
   protected String getModelInfos(WorldModelB worldModelB, boolean detailed) {
+//    Thread t = new Thread(() -> {
       try {
         String filename = model.dumpAst(builder -> {
           builder.excludeChildren("Orientation", "Size");
@@ -183,6 +175,9 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
       } catch (Exception e) {
         logger.catching(e);
       }
+//    });
+//    t.setDaemon(true);
+//    t.start();
 
     return UtilB.getModelInfos(model, detailed);
   }