From 0d84a0cd4cb48991e543805e19b9faae2ea79086 Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Wed, 15 Jun 2022 16:27:09 +0200
Subject: [PATCH] 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
---
 ...ros3rag.java-ragconnect-conventions.gradle |   3 +-
 .../src/main/jastadd/WorldModelB.connect      |  19 +++
 .../src/main/jastadd/WorldModelB.jadd         | 116 +++++++++++++++---
 .../de/tudresden/inf/st/placeB/MainB.java     |  81 ++++++------
 4 files changed, 159 insertions(+), 60 deletions(-)

diff --git a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle
index 9a79e8a..f4c8f22 100644
--- a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle
+++ b/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'
 }
-
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index da2da8d..302541d 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/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 {
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index 704964c..2c5a092 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/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 ---
@@ -390,24 +410,24 @@ aspect AttributeMappings {
     throw new RuntimeException("Separate Place operation not supported yet!");
   }
   eq PickAndPlace.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
-        .setPickAndPlace(de.tudresden.inf.st.ceti.PickAndPlace.newBuilder()
+      .setPickAndPlace(de.tudresden.inf.st.ceti.PickAndPlace.newBuilder()
           .setIdRobot(getRobotToExecute().getName())
           .setIdPick(getObjectToPick().getName())
           .setIdPlace(getTargetLocation().getName())
           .build())
-        .build().toByteArray();
+      .build().toByteArray();
   eq ConfigChange.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
-        .setConfigChange(de.tudresden.inf.st.ceti.ConfigChange.newBuilder()
+      .setConfigChange(de.tudresden.inf.st.ceti.ConfigChange.newBuilder()
           .setIdRobotNewOwner(getRobotToExecute().getName())
           .setIdCollaborationZone(getCollaborationZone().getName())
           .build())
-        .build().toByteArray();
+      .build().toByteArray();
   eq Evacuate.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder()
-        .setEvacuate(de.tudresden.inf.st.ceti.Evacuate.newBuilder()
+      .setEvacuate(de.tudresden.inf.st.ceti.Evacuate.newBuilder()
           .setIdRobot(getRobotToExecute().getName())
           .setIdCollaborationZone(getCollaborationZone().getName())
           .build())
-        .build().toByteArray();
+      .build().toByteArray();
   eq Wait.toProtobufByteArray() = null;
 }
 
@@ -427,11 +447,45 @@ 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();
-//  eq WorldModelB.getChild().allMappings() = getLocationMapping();
+  //  inh LocationMapping LogicalDropOffLocation.allMappings();
+  //  eq WorldModelB.getChild().allMappings() = getLocationMapping();
   syn boolean CollaborationZone.hasOccupient() = occupient() != null;
   syn Robot CollaborationZone.occupient() {
     // (>) == moving in, (^) == evacuate/moving out
@@ -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) {
@@ -564,8 +650,8 @@ aspect Resolving {
     return null;
   }
 
-//  // do not resolve real regions and real locations (send by site-A)
-//  refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null;
+  //  // do not resolve real regions and real locations (send by site-A)
+  //  refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null;
 }
 aspect RagConnectAddOn {
   public void WorldModelB.ragconnectResetEvaluationCounter() {
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
index 7e3a199..9df4a09 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
+++ b/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,18 +162,22 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
 
   @Override
   protected String getModelInfos(WorldModelB worldModelB, boolean detailed) {
-    try {
-      String filename = model.dumpAst(builder -> {
-        builder.excludeChildren("Orientation", "Size");
-        builder.excludeRelations("ContainedInRegion");
-        builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations");
-        builder.includeAttributes("realRegion", "computeOperations", "myPosition");
-        builder.includeNullNodes();
-      });
-      mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8));
-    } catch (Exception e) {
-      logger.catching(e);
-    }
+//    Thread t = new Thread(() -> {
+      try {
+        String filename = model.dumpAst(builder -> {
+          builder.excludeChildren("Orientation", "Size");
+          builder.excludeRelations("ContainedInRegion");
+          builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations");
+          builder.includeAttributes("realRegion", "computeOperations", "myPosition");
+          builder.includeNullNodes();
+        });
+        mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8));
+      } catch (Exception e) {
+        logger.catching(e);
+      }
+//    });
+//    t.setDaemon(true);
+//    t.start();
 
     return UtilB.getModelInfos(model, detailed);
   }
-- 
GitLab