diff --git a/build.gradle b/build.gradle
index 1d0c641eb4ab30b6544c6edbeb1890438d8cbf08..6bcf15e536e07fb19f008c40a35f2cb14b9895b8 100644
--- a/build.gradle
+++ b/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"
diff --git a/src/main/jastadd/cleanup/Serialization.jadd b/src/main/jastadd/cleanup/Serialization.jadd
new file mode 100644
index 0000000000000000000000000000000000000000..cfb8dc4629404e38d8e4638255053737d823938d
--- /dev/null
+++ b/src/main/jastadd/cleanup/Serialization.jadd
@@ -0,0 +1,19 @@
+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(); }
+}
diff --git a/src/main/jastadd/common/Tracing.jadd b/src/main/jastadd/common/Tracing.jadd
index cce799e68e276a4a8fe2a0f922355f6cca0bd95c..bc807152f65e6c8e3077ce840a81a6827bbd60fc 100644
--- a/src/main/jastadd/common/Tracing.jadd
+++ b/src/main/jastadd/common/Tracing.jadd
@@ -1,15 +1,33 @@
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;
- try {
- world = highlightNode.containingWorld();
- } catch (NullPointerException e) {
- // the element is not in a full AST (yet?), so we don't print it
- return;
+ 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();
@@ -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();
diff --git a/src/main/java/de/tudresden/inf/st/mg/Main.java b/src/main/java/de/tudresden/inf/st/mg/Main.java
index 872da41b3fdb95c189ec5f5b4010f7cc97681f0b..9dcea6127799f2aab18d3a6c853777d222cca781 100644
--- a/src/main/java/de/tudresden/inf/st/mg/Main.java
+++ b/src/main/java/de/tudresden/inf/st/mg/Main.java
@@ -1,12 +1,23 @@
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);
diff --git a/src/main/java/de/tudresden/inf/st/mg/common/ColorDeserializer.java b/src/main/java/de/tudresden/inf/st/mg/common/ColorDeserializer.java
new file mode 100644
index 0000000000000000000000000000000000000000..437aac189abb0b73fcaa27f737eae2040422055d
--- /dev/null
+++ b/src/main/java/de/tudresden/inf/st/mg/common/ColorDeserializer.java
@@ -0,0 +1,32 @@
+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
+ }
+}
diff --git a/src/main/java/de/tudresden/inf/st/mg/common/TableDeserializer.java b/src/main/java/de/tudresden/inf/st/mg/common/TableDeserializer.java
new file mode 100644
index 0000000000000000000000000000000000000000..72a54608072b57c25387c3b0f5193ea57bed5132
--- /dev/null
+++ b/src/main/java/de/tudresden/inf/st/mg/common/TableDeserializer.java
@@ -0,0 +1,61 @@
+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;
+ }
+}
diff --git a/src/main/resources/de/tudresden/inf/st/mg/config_scene_virtual-table.yaml b/src/main/resources/de/tudresden/inf/st/mg/config_scene_virtual-table.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..642d9e21d345c32356eb33f022020e9ff908ad21
--- /dev/null
+++ b/src/main/resources/de/tudresden/inf/st/mg/config_scene_virtual-table.yaml
@@ -0,0 +1,25 @@
+# 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': { } }
+] }
diff --git a/src/test/java/de/tudresden/inf/st/mg/ParserTest.java b/src/test/java/de/tudresden/inf/st/mg/ParserTest.java
index 35414ad9224b3d71b8397bbf3f02d1e8d313d4d7..9ad07f03f28c133a1b32401d173b0f6f7edcc2e2 100644
--- a/src/test/java/de/tudresden/inf/st/mg/ParserTest.java
+++ b/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