load config scene from file

build.gradle

@@ -54,6 +54,7 @@ dependencies {
     implementation group: 'org.json', name: 'json', version: '20220320'
     implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: '2.13.3'
     implementation group: 'com.fasterxml.jackson.core', name: 'jackson-core', version: '2.13.3'
+    implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: '2.13.3'
     implementation group: 'org.slf4j', name: 'slf4j-simple', version: '1.7.31'
     ragconnect group: 'de.tudresden.inf.st', name: 'ragconnect', version: '1.0.0-alpha-203'
     jastadd2 "org.jastadd:jastadd2:2.3.5-dresden-6"

src/main/jastadd/cleanup/Serialization.jadd

+aspect Serialization {
+
+  // @com.fasterxml.jackson.annotation.JsonIgnoreProperties(ignoreUnknown = true)
+  @com.fasterxml.jackson.annotation.JsonIgnoreProperties({"type", "pos", "size", "orientation"})
+  interface PhysicalObjectInterface {}
+  PhysicalObject implements PhysicalObjectInterface;
+
+  refine
+  @com.fasterxml.jackson.annotation.JsonGetter("objects")
+  public JastAddList<PhysicalObject> Table.getPhysicalObjectList() { return refined(); }
+
+  refine
+  @com.fasterxml.jackson.annotation.JsonGetter("id")
+  public String PhysicalObject.getName() { return refined(); }
+
+  refine
+  @com.fasterxml.jackson.databind.annotation.JsonDeserialize(using = de.tudresden.inf.st.mg.common.ColorDeserializer.class)
+  public String PhysicalObject.getColor() { return refined(); }
+}

src/main/jastadd/common/Tracing.jadd

 aspect Tracing {
 
+  private static boolean World.printContext = false;
+
+  public static void World.enableContextPrinting(boolean printContext) {
+    World.printContext = printContext;
+  }
+
+  public void World.printContext(String step) {
+    World.printContextOf(step, this, de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
+  }
+
   synchronized public static void World.printContextOf(String step, ASTNode<?> highlightNode, String highlightColor) {
 
+    if (!printContext) {
+      return;
+    }
+
     final String DEFAULT_COLOR = de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR;
 
     World world;
+    if (highlightNode instanceof World) {
+      world = (World) highlightNode;
+    } else {
       try {
         world = highlightNode.containingWorld();
       } catch (NullPointerException e) {
         // the element is not in a full AST (yet?), so we don't print it
         return;
       }
+    }
     String worldName = world.getClass().getSimpleName();
 
     try {
@@ -21,7 +39,6 @@ aspect Tracing {
           .includeAttributeWhen((node, attributeName, isNTA, value) -> isNTA)
           .<ASTNode>includeChildWhen((parentNode, childNode, contextName) -> !((contextName.equals("Pose") || contextName.equals("Robot")) && childNode.inDemonstrationTable()))
           .<ASTNode>includeRelationsWhen((sourceNode, targetNode, roleName) -> !roleName.equals("AttachedRobot") && !(sourceNode != null && sourceNode.inDemonstrationTable()) && !(targetNode != null && targetNode.inDemonstrationTable())) // !sourceNode.inDemonstrationTable()
-//          .excludeRelations("AttachedRobot")
           .dumpAsSVG(de.tudresden.inf.st.mg.common.MotionGrammarConfig.astDiagramDir.resolve("Context-" + worldName + "-" + new java.text.SimpleDateFormat("yyyy.MM.dd.HH.mm.ss.SSS").format(new java.util.Date()) + "-" + step + ".svg"));
     } catch (java.io.IOException e) {
       e.printStackTrace();

src/main/java/de/tudresden/inf/st/mg/Main.java

 package de.tudresden.inf.st.mg;
 
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.module.SimpleModule;
+import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
+import com.google.protobuf.util.JsonFormat;
+import de.tudresden.inf.st.ceti.Scene;
 import de.tudresden.inf.st.mg.common.MotionGrammarConfig;
 import de.tudresden.inf.st.mg.common.MotionGrammarParser;
+import de.tudresden.inf.st.mg.common.TableDeserializer;
 import de.tudresden.inf.st.mg.jastadd.model.RobotWorld;
+import de.tudresden.inf.st.mg.jastadd.model.Table;
+import de.tudresden.inf.st.mg.jastadd.model.World;
 
+import java.io.File;
 import java.io.IOException;
 import java.net.URISyntaxException;
+import java.nio.file.Files;
 import java.nio.file.Path;
+import java.util.Comparator;
 
 public class Main {
 
@@ -19,9 +30,28 @@ public class Main {
   public static void main(String[] args) {
 
     MotionGrammarConfig.astDiagramDir = TIDY_AST_DIAGRAM_DIR;
+    try {
+      Files.walk(TIDY_AST_DIAGRAM_DIR).sorted(Comparator.reverseOrder()).map(Path::toFile).forEach(File::delete);
+      Files.createDirectories(TIDY_AST_DIAGRAM_DIR);
+    } catch (IOException e) {
+      // do nothing
+    }
 
     RobotWorld world = RobotWorld.initialWorld();
 
+    try {
+      SimpleModule module = new SimpleModule();
+      module.addDeserializer(Table.class, new TableDeserializer());
+      ObjectMapper mapper = new ObjectMapper(new YAMLFactory());
+      mapper.registerModule(module);
+      world.setDemonstrationTable(mapper.readValue(Main.class.getResourceAsStream("config_scene_virtual-table.yaml"), Table.class));
+    } catch (IOException ignored) {
+      ignored.printStackTrace();
+    }
+
+    World.enableContextPrinting(true);
+    world.printContext("initial");
+
     org.fusesource.mqtt.client.MQTT handler = new org.fusesource.mqtt.client.MQTT();
     try {
       handler.setHost("tcp://localhost:1883");
@@ -43,14 +73,6 @@ public class Main {
       e.printStackTrace();
     }
 
-    while (world.getTable().getNumPhysicalObject() == 0) {
-      try {
-        Thread.sleep(1000);
-      } catch (InterruptedException e) {
-        e.printStackTrace();
-      }
-    }
-
     // create a parser using the world
     RobotParser parser = new RobotParser(world);
 

src/main/java/de/tudresden/inf/st/mg/common/ColorDeserializer.java

+package de.tudresden.inf.st.mg.common;
+
+import com.fasterxml.jackson.core.JacksonException;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.TreeNode;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonDeserializer;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.node.ValueNode;
+import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
+import de.tudresden.inf.st.mg.jastadd.model.Bin;
+import de.tudresden.inf.st.mg.jastadd.model.MovableObject;
+import de.tudresden.inf.st.mg.jastadd.model.Robot;
+import de.tudresden.inf.st.mg.jastadd.model.Table;
+
+import java.io.IOException;
+
+public class ColorDeserializer extends JsonDeserializer<String> {
+    @Override
+    public String deserialize(JsonParser p, DeserializationContext ctxt) throws IOException {
+        JsonNode node = p.getCodec().readTree(p);
+        JsonNode nodeR = node.get("r");
+        Long r = nodeR == null ? 0 : Math.round(nodeR.asDouble() * 255);
+        JsonNode nodeG = node.get("g");
+        Long g = nodeG == null ? 0 : Math.round(nodeG.asDouble() * 255);
+        JsonNode nodeB = node.get("b");
+        Long b = nodeB == null ? 0 : Math.round(nodeB.asDouble() * 255);
+        System.out.println("#" + String.format("%02X%02X%02X", r, g, b));
+        return "\"#" + String.format("%02X%02X%02X", r, g, b) + "\""; // FIXME remove quotes
+    }
+}

src/main/java/de/tudresden/inf/st/mg/common/TableDeserializer.java

+package de.tudresden.inf.st.mg.common;
+
+import com.fasterxml.jackson.core.JacksonException;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonToken;
+import com.fasterxml.jackson.core.TreeNode;
+import com.fasterxml.jackson.databind.*;
+import com.fasterxml.jackson.databind.module.SimpleModule;
+import com.fasterxml.jackson.databind.node.ValueNode;
+import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
+import de.tudresden.inf.st.mg.jastadd.model.*;
+
+import java.io.IOException;
+
+public class TableDeserializer extends JsonDeserializer<Table> {
+
+    // idea taken from https://gist.github.com/sverhagen/c199d924245a078c287b46546e1ac26c
+    // TODO this looks strange, since we should already have an ObjectMapper
+    private static final ObjectMapper OBJECT_MAPPER = new ObjectMapper(new YAMLFactory());
+
+    @Override
+    public Table deserialize(JsonParser p, DeserializationContext ctxt) throws IOException, JacksonException {
+        JsonNode node = p.getCodec().readTree(p);
+
+        Table result = new Table();
+        result.setRobot(new Robot());
+
+        if (node.isObject() && node.get("objects").isArray()) {
+            for (TreeNode child : node.get("objects")) {
+                TreeNode typeNode = child.get("type");
+                String type = "UNKNOWN";
+                if (typeNode != null && typeNode.isValueNode()) {
+                    type = ((ValueNode) typeNode).asText();
+                }
+                switch (type) {
+                    case "BIN":
+                        Bin bin = OBJECT_MAPPER.convertValue(child, Bin.class);
+                        bin.setPose(new Pose(0,0,0,0,0,0,1)); //TODO use real pose
+                        result.addPhysicalObject(bin);
+                        break;
+                    case "BOX":
+                        MovableObject movableObject = OBJECT_MAPPER.convertValue(child, MovableObject.class);
+                        movableObject.setPose(new Pose(0,0,0,0,0,0,1)); //TODO use real pose
+                        result.addPhysicalObject(movableObject);
+                        break;
+                    case "ARM":
+                    case "UNKNOWN":
+                        // ignore the arm
+                        break;
+                    default:
+                        System.out.println("ignoring scene content of type " + type);
+                }
+
+            }
+        } else {
+            throw new IOException("YAML node is not an object with an 'objects' array.");
+        }
+
+        return result;
+    }
+}

src/main/resources/de/tudresden/inf/st/mg/config_scene_virtual-table.yaml

+# create json file using
+# yq eval -o=json config_scene_virtual-table.yaml > config_scene_virtual-table.json
+{ 'objects': [
+  # table
+  # height: 1m
+  # width: 2m
+  { 'id': 'table', 'pos': { 'z': 0.25 },'size': { 'length': 1,'width': 2,'height': .5 },'orientation': { 'w': 1 },'color': { 'r': 0.3,'g': 0.3,'b': 0.3 } },
+
+  # idea: all elements are sorted by name
+  { 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.35,'y': -0.60,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
+  { 'id': 'binGreen',  'type': 'BIN','pos': { 'x': -0.35,'y': 0,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
+  { 'id': 'binRed','type': 'BIN','pos': { 'x': -0.35,'y': 0.60,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
+
+  { 'id': 'bigBlue',  'type': 'BOX','pos': { 'x': 0.2,'y': -0.300000,'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
+  { 'id': 'bigGreen', 'type': 'BOX','pos': { 'x': 0.2,'y': -0.100000,'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
+  { 'id': 'bigRed',   'type': 'BOX','pos': { 'x': 0.2,'y': 0.100000, 'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
+  { 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.2,'y': 0.300000, 'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'r': 0.5, 'g': 0.5 } },
+
+  { 'id': 'smallBlue',  'type': 'BOX','pos': { 'x': 0.4,'y': -0.200000,'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
+  { 'id': 'smallGreen', 'type': 'BOX','pos': { 'x': 0.4,'y': 0.000000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
+  { 'id': 'smallRed',   'type': 'BOX','pos': { 'x': 0.4,'y': 0.200000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
+  { 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.4,'y': 0.400000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'r': 0.5, 'g': 0.5 } },
+
+  { 'id': 'arm','type': 'ARM','pos': { },'size': { },'orientation': { 'w': 1 },'color': { } }
+] }

src/test/java/de/tudresden/inf/st/mg/ParserTest.java

@@ -33,6 +33,8 @@ public class ParserTest {
     }
     Files.createDirectories(LOAD_AST_DIAGRAM_DIR);
     Files.createDirectories(TIDY_AST_DIAGRAM_DIR);
+
+    World.enableContextPrinting(true);
   }
 
   @Test