initial version of motion grammar working with real robot

build.gradle

@@ -3,6 +3,7 @@ plugins {
     id 'application'
     id 'idea'
     id 'org.jastadd' version "${jastaddgradle_version}"
+    id "com.google.protobuf" version "${protobuf_plugin_version}"
 }
 
 group 'de.tudresden.inf.st'
@@ -16,6 +17,13 @@ application.mainClass = "${mainClassName}"
 
 java.toolchain.languageVersion = JavaLanguageVersion.of(11)
 
+protobuf {
+    protoc {
+        // The artifact spec for the Protobuf Compiler
+        artifact = "com.google.protobuf:protoc:${protobuf_version}"
+    }
+}
+
 repositories {
     mavenCentral()
     maven {
@@ -28,6 +36,7 @@ configurations {
     // add a configuration to store the grammar printing dependency in
     grammar2uml
     relast
+    ragconnect
 }
 
 File genSrc = file("src/gen/java")
@@ -46,6 +55,11 @@ dependencies {
     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: '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"
+    implementation group: 'com.google.protobuf', name: 'protobuf-java', version: "${protobuf_version}"
+    implementation group: 'com.google.protobuf', name: 'protobuf-java-util', version: "${protobuf_version}"
+    implementation group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
 }
 
 def versionFile = 'src/main/resources/Version.properties'
@@ -97,6 +111,33 @@ def loadingGrammarDiagramFile = './src/gen/resources/diagrams/grammar/loading.pn
 def cleaningGrammarDiagramFile = './src/gen/resources/diagrams/grammar/cleaning.png'
 def jastAddListName = 'JastAddList'
 
+// phases: ragConnect -> RelAst -> JastAdd
+// phase: ragConnect
+task ragConnect(type: JavaExec) {
+    group = 'Build'
+    main = 'org.jastadd.ragconnect.compiler.Compiler'
+    classpath = configurations.ragconnect
+
+    args([
+            '--o=src/gen/jastadd',
+            './src/main/jastadd/common/MotionGrammar.relast',
+            './src/main/jastadd/cleanup/RobotWorld.relast',
+            './src/main/jastadd/cleanup/RobotWorld.connect',
+            '--logReads',
+            '--logWrites',
+//            '--logIncremental',
+            '--verbose',
+            '--rootNode=RobotScene',
+            '--protocols=mqtt',
+            "--List=${jastAddListName}",
+            '--experimental-jastadd-329',
+            '--incremental=param',
+            '--tracing=cache,flush',
+            '--evaluationCounter'
+    ])
+
+}
+
 task generateLoadingGrammarDiagrams(type: JavaExec) {
     group = 'Documentation'
     classpath = configurations.grammar2uml
@@ -128,6 +169,7 @@ task relastToJastAdd(type: JavaExec) {
             '--useJastAddNames',
             '--listClass=java.util.ArrayList',
             "--jastAddList=${jastAddListName}",
+            "--resolverHelper",
             '--file'
     args relastFiles
 
@@ -170,11 +212,15 @@ jastadd {
     buildInfoDir = 'src/gen-res'
 
     // default options are: '--rewrite=cnta', '--safeLazy', '--visitCheck=false', '--cacheCycle=false'
-    extraJastAddOptions = ["--List=${jastAddListName}"]
+    extraJastAddOptions = ["--List=${jastAddListName}",
+                           "--flush=api",
+                           "--incremental=param,debug",
+                           "--tracing=cache,flush"]
 }
 
 // Workflow configuration for phases
 clean.dependsOn cleanGen
 generateAst.dependsOn relastToJastAdd
 generateAst.dependsOn generateLoadingGrammarDiagrams, generateCleaningGrammarDiagrams
+relastToJastAdd.dependsOn ragConnect
 

gradle.properties

@@ -5,3 +5,5 @@ assertj_version = 3.22.0
 grammar2uml_version = 0.2.1
 jastaddgradle_version = 1.14.5
 javalin_version = 4.6.3
+protobuf_version = 4.0.0-rc-2
+protobuf_plugin_version = 0.8.18

src/main/jastadd/cleanup/RobotWorld.connect

+// --- receiving ---
+receive RobotScene.Table using ParseScene, ConvertScene ;
+
+ParseScene maps byte[] bytes to de.tudresden.inf.st.ceti.Scene {:
+  return de.tudresden.inf.st.ceti.Scene.parseFrom(bytes);
+:}
+
+ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Table {:
+  var result = new Table();
+  result.setRobot(new Robot());
+  for (var pbObject : pbScene.getObjectsList()) {
+    PhysicalObject obj;
+    if (pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX || pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BIN) {
+      PhysicalObject physicalObject = pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX ? new MovableObject() : new Bin();
+      physicalObject.setName(pbObject.getId());
+      java.awt.Color color = new java.awt.Color(pbObject.getColor().getR(), pbObject.getColor().getG(), pbObject.getColor().getB());
+      physicalObject.setColor("#" + Integer.toHexString(color.getRGB()).substring(2));
+      Pose pose = new Pose();
+      pose.setX(pbObject.getPos().getX());
+      pose.setY(pbObject.getPos().getY());
+      pose.setZ(pbObject.getPos().getZ());
+      pose.setQX(pbObject.getOrientation().getX());
+      pose.setQY(pbObject.getOrientation().getY());
+      pose.setQZ(pbObject.getOrientation().getZ());
+      pose.setQW(pbObject.getOrientation().getW());
+      physicalObject.setPose(pose);
+      if (pbObject.getOwner() != "" && pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX) {
+        ((MovableObject) physicalObject).setAttachedRobot(result.getRobot());
+      }
+      result.addPhysicalObject(physicalObject);
+    } else if (pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.ARM) {
+        result.getRobot().setIsIdle(pbObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE);
+    }
+  }
+  return result;
+:}

src/main/jastadd/cleanup/RobotWorld.jadd

 aspect RobotWorld {
 
-  public static RobotScene RobotScene.initialWorld(java.util.Random r) {
+  public boolean RobotScene.simulated = true;
+  public org.fusesource.mqtt.client.BlockingConnection RobotScene.connection;
+
+  public static RobotScene RobotScene.initialWorld(java.util.Random random) {
     Table t = new Table();
     t.addPhysicalObject(new Bin("binRed", Pose.of(-10, -10, 0), "red"));
     t.addPhysicalObject(new Bin("binGreen", Pose.of(-10, 0, 0), "green"));
     t.addPhysicalObject(new MovableObject("boxRed", Pose.of(-5, -10, 0), "red"));
     t.addPhysicalObject(new MovableObject("boxGreen", Pose.of(-5, 0, 0), "green"));
-    Robot b = new Robot();
-    b.setIsIdle(true);
-    return new RobotScene(t, b);
+    Robot r = new Robot();
+    r.setIsIdle(true);
+    t.setRobot(r);
+    return new RobotScene(t);
+  }
+
+  public static RobotScene RobotScene.initialWorld() {
+    Robot r = new Robot();
+    r.setIsIdle(true);
+    Table t = new Table();
+    t.setRobot(r);
+    RobotScene result = new RobotScene(t);
+    result.simulated = false;
+    return result;
   }
 
   public static TokenType EmptyTable.type()                 { return TokenType.of("EMPTY_TABLE"); }
@@ -151,7 +165,7 @@ aspect RobotWorld {
   }
   public RobotIsIdle RobotScene.parseRobotIsIdle() {
     System.out.print("Trying to parse token <RobotIsIdle>... ");
-    if (getRobot().getIsIdle()) {
+    if (getTable().getRobot().getIsIdle()) {
       System.out.println("success");
       return new RobotIsIdle();
     } else {
@@ -165,7 +179,7 @@ aspect RobotWorld {
   }
   public RobotIsNotIdle RobotScene.parseRobotIsNotIdle() {
     System.out.print("Trying to parse token <RobotIsNotIdle>... ");
-    if (!getRobot().getIsIdle()) {
+    if (!getTable().getRobot().getIsIdle()) {
       System.out.println("success");
       return new RobotIsNotIdle();
     } else {
@@ -179,7 +193,7 @@ aspect RobotWorld {
   }
   public RobotHasItemAttached RobotScene.parseRobotHasItemAttached() {
     System.out.print("Trying to parse token <RobotHasItemAttached>... ");
-    if (getRobot().hasAttachedItem()) {
+    if (getTable().getRobot().hasAttachedItem()) {
       System.out.println("success");
       return new RobotHasItemAttached();
     } else {
@@ -193,7 +207,7 @@ aspect RobotWorld {
   }
   public RobotHasNoItemAttached RobotScene.parseRobotHasNoItemAttached() {
     System.out.print("Trying to parse token <RobotHasNoItemAttached>... ");
-    if (!getRobot().hasAttachedItem()) {
+    if (!getTable().getRobot().hasAttachedItem()) {
       System.out.println("success");
       return new RobotHasNoItemAttached();
     } else {

src/main/jastadd/cleanup/RobotWorld.relast

 // World Model of the Motion Grammar
 
-RobotScene : World ::=  Table Robot;
+RobotScene : World ::= Table;
 
-Table ::= PhysicalObject*;
+Table ::= PhysicalObject* Robot;
 abstract PhysicalObject ::= <Name> Pose <Color>;
 MovableObject : PhysicalObject;
 Bin : PhysicalObject;

src/main/jastadd/cleanup/SemanticActions.jadd

 aspect SemanticActions {
 
-  public void PickUpObject.action(World world) {
-    RobotScene scene = world.asRobotScene();
-    System.out.println("performing semantic action for element Pick");
-    String objectName = object();
-    java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
-      scene.getRobot().setIsIdle(false);
-      try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
-      MovableObject object = scene.getTable().getMovableObjectByName(objectName);
-      scene.getRobot().setAttachedItem(object);
-      object.setPose(Pose.of(-1,-1,-1));
-      try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
-      scene.getRobot().setIsIdle(true);
-    });
-  }
+    public void PickUpObject.action(World world) {
+        RobotScene scene = world.asRobotScene();
+        System.out.println("performing semantic action for element Pick");
+        String objectName = object();
 
-  public void DropObjectAtRightPlace.action(World world) {
-    RobotScene scene = world.asRobotScene();
-    System.out.println("performing semantic action for element Pick");
-    String objectName = object();
-    String placeName = place();
-    java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
-      scene.getRobot().setIsIdle(false);
-      try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
-      MovableObject object = scene.getTable().getMovableObjectByName(objectName);
-      Bin bin = scene.getTable().getBinByName(placeName);
-      object.setPose(Pose.of(bin.getPose()));
-      scene.getRobot().setAttachedItem(null);
-      try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
-      scene.getRobot().setIsIdle(true);
-    });
-  }
+        if (scene.simulated) {
+            java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
+                scene.getTable().getRobot().setIsIdle(false);
+                try {
+                    Thread.sleep(1200);
+                } catch (InterruptedException e) { /* ignore */ }
+                MovableObject object = scene.getTable().getMovableObjectByName(objectName);
+                scene.getTable().getRobot().setAttachedItem(object);
+                object.setPose(Pose.of(-1, -1, -1));
+                try {
+                    Thread.sleep(1200);
+                } catch (InterruptedException e) { /* ignore */ }
+                scene.getTable().getRobot().setIsIdle(true);
+            });
+        } else {
+            de.tudresden.inf.st.ceti.Command c = de.tudresden.inf.st.ceti.Command.newBuilder().setPick(de.tudresden.inf.st.ceti.Pick.newBuilder().setIdRobot("arm").setIdPick(object()).build()).build();
+            try {
+                scene.connection.publish("/ceti_cell_placeworld/command", c.toByteArray(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false);
+            } catch (Exception e) {
+                e.printStackTrace();
+            }
+        }
+    }
+
+    public void DropObjectAtRightPlace.action(World world) {
+        RobotScene scene = world.asRobotScene();
+        System.out.println("performing semantic action for element Pick");
+        String objectName = object();
+        String placeName = place();
+        if (scene.simulated) {
+            java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
+                scene.getTable().getRobot().setIsIdle(false);
+                try {
+                    Thread.sleep(1200);
+                } catch (InterruptedException e) { /* ignore */ }
+                MovableObject object = scene.getTable().getMovableObjectByName(objectName);
+                Bin bin = scene.getTable().getBinByName(placeName);
+                object.setPose(Pose.of(bin.getPose()));
+                scene.getTable().getRobot().setAttachedItem(null);
+                try {
+                    Thread.sleep(1200);
+                } catch (InterruptedException e) { /* ignore */ }
+                scene.getTable().getRobot().setIsIdle(true);
+            });
+        } else {
+            de.tudresden.inf.st.ceti.Command c = de.tudresden.inf.st.ceti.Command.newBuilder().setDrop(de.tudresden.inf.st.ceti.Drop.newBuilder().setIdRobot("arm").setIdBin(place()).build()).build();
+            try {
+                scene.connection.publish("/ceti_cell_placeworld/command", c.toByteArray(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false);
+            } catch (Exception e) {
+                e.printStackTrace();
+            }
+        }
+    }
 
-  public void Wait.action(World world) {
-    RobotScene container = world.asRobotScene();
-    System.out.println("performing semantic action for element Wait");
-    try {
-      Thread.sleep((long) (time() * 1000));
-    } catch (InterruptedException e) {
-      // ignore
+    public void Wait.action(World world) {
+        RobotScene container = world.asRobotScene();
+        System.out.println("performing semantic action for element Wait");
+        try {
+            Thread.sleep((long) (time() * 1000));
+        } catch (InterruptedException e) {
+            // ignore
+        }
     }
-  }
 
 }

src/main/jastadd/cleanup/Tracing.jadd

@@ -24,21 +24,30 @@ aspect Tracing {
     World.printContextOf("Pose.setZ()-DONE", this, de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
   }
 
+  refine
+  public RobotScene RobotScene.setTable(Table t) {
+    refined(t);
+    World.printContextOf("RobotScene.setTable()-DONE", getTable(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
+    return this;
+  }
+
   // this is probably never called because each child overrides the setter method
   refine
-  public void PhysicalObject.setPose(Pose p) {
+  public PhysicalObject PhysicalObject.setPose(Pose p) {
     World.printContextOf("PhysicalObject.setPose()-BEFORE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.REMOVE_COLOR);
     refined(p);
     World.printContextOf("PhysicalObject.setPose()-AFTER", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.ADD_COLOR);
     World.printContextOf("PhysicalObject.setPose()-DONE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
+    return this;
   }
 
   refine
-  public void MovableObject.setPose(Pose p) {
+  public MovableObject MovableObject.setPose(Pose p) {
     World.printContextOf("MovableObject.setPose()-BEFORE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.REMOVE_COLOR);
     refined(p);
     World.printContextOf("MovableObject.setPose()-AFTER", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.ADD_COLOR);
     World.printContextOf("MovableObject.setPose()-DONE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
+    return this;
   }
 
   refine

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

@@ -65,8 +65,7 @@ public abstract class MotionGrammarParser<T extends MotionGrammarElement> {
           .customPreamble("title " + step)
           .skinParam(SkinParamBooleanSetting.Shadowing, false)
           .skinParam(SkinParamStringSetting.backgroundColor, "white")
-          .dumpAsSVG(svgPath)
-          .dumpAsPNG(MotionGrammarConfig.astDiagramDir.resolve("AST-" + String.format("%03d", timeStep_) + "-" + new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss.SSS").format(new java.util.Date()) + "-" + step + ".png"));
+          .dumpAsSVG(svgPath);
       DiagramProvider.getInstance().publishAst(new Date(), timeStep_, step, svgPath);
       timeStep_++;
     } catch (IOException e) {

src/main/proto/connector.proto

+// connector.proto
+// this file contains the messages that are exchanged between the ROS connector and other systems
+
+syntax = "proto3";
+
+option java_package = "de.tudresden.inf.st.ceti";
+option java_multiple_files = true;
+
+message Object {
+
+    // Position is object-center related
+    message Position {
+        double x = 1; // in m
+        double y = 2; // in m
+        double z = 3; // height in m
+    }
+
+    // 3D description of the object
+    message Size {
+        double length = 1; // in m
+        double width = 2;  // in m
+        double height = 3; // in m
+    }
+    message Orientation {
+        double x = 1; // normalized quaternion
+        double y = 2;
+        double z = 3;
+        double w = 4;
+    }
+    message Color {
+        float r = 1; // 0..1
+        float g = 2; // 0..1
+        float b = 3; // 0..1
+    }
+    enum Type {
+        UNKNOWN = 0;
+        BOX = 1;
+        BIN = 2;
+        ARM = 3;
+        DROP_OFF_LOCATION = 4;
+        HUMAN = 5;
+        ROBOT = 6;
+        COLLABORATION_ZONE = 7;
+    }
+    enum State {
+        STATE_UNKNOWN = 0;
+
+        // robot states
+        STATE_IDLE = 101;
+        STATE_PICKING = 102;
+        STATE_PLACING = 103;
+        STATE_MOVING = 104;
+
+        // object
+        STATE_STATIONARY = 200;
+        STATE_PICKED = 201;
+    }
+
+    // determines the meaning of a scene object when the scene has is_delta flag
+    enum Mode {
+        MODE_UNKNOWN = 0; // if and only if is_delta = false, this should be the value of the field
+        MODE_KEEP = 1;    // the object's properties do not change, thus don't need to be processed by the receiver
+        MODE_MODIFY = 2;  // the object's properties have changed
+        MODE_ADD = 3;     // the object did not exist in the scene before
+        MODE_REMOVE = 4;  // the object is removed from the scene, its properties can be ignored
+    }
+
+    string id = 1;
+    Type type = 2;
+    Position pos = 3;
+    Size size = 4;
+    Orientation orientation = 5;
+    Color color = 6;
+    State state = 7;
+    string owner = 8;
+    Mode mode = 9;
+}
+
+// the scene is stored within the ROS side and sent to clients
+message Scene {
+    repeated Object objects = 1;
+    bool is_delta = 2;
+}
+
+// the selection is done by a client and sent to ROS
+message Selection {
+    string id = 1; // the id corresponds to an id of an Object in a Scene
+}
+
+message Command {
+    oneof msg {
+        PickAndPlace pickAndPlace = 1;
+        ConfigChange configChange = 2;
+        Evacuate evacuate = 3;
+        Pick pick = 4;
+        Place place = 5;
+        Drop drop = 6;
+    }
+}
+
+message PickAndPlace {
+    string idRobot = 1;  // id of the robot that should execute this operation
+    string idPick = 2;  // id of the object in the scene to be picked
+    string idPlace = 3;  // id of the location the picked object shall be placed.
+}
+
+message ConfigChange {
+    string idCollaborationZone = 1;  // id of collaboration zone to change
+    string idRobotNewOwner = 2;  // id of robot that will become new owner
+}
+
+message Evacuate {
+    string idRobot = 1;  // id of robot that need to move out of its currently defined collision objects
+    string idCollaborationZone = 2;  // id of collaboration zone to evacuate
+}
+
+message Pick {
+    string idRobot = 1;  // id of the robot that should execute this operation
+    string idPick = 2;  // id of the object in the scene to be picked
+}
+
+message Place {
+    string idRobot = 1;  // id of the robot that should execute this operation
+    string idPlace = 2;  // id of the location currently grasped object shall be placed.
+}
+
+message Drop {
+    string idRobot = 1;  // id of the robot that should execute this operation
+    string idBin = 2;  // id of the bin the picked object shall be dropped in.
+}
\ No newline at end of file

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

 package de.tudresden.inf.st.mg;
 
+import de.tudresden.inf.st.mg.common.MotionGrammarConfig;
 import de.tudresden.inf.st.mg.common.MotionGrammarParser;
 import de.tudresden.inf.st.mg.jastadd.model.*;
 import org.junit.jupiter.api.BeforeAll;
@@ -8,10 +9,12 @@ import org.junit.jupiter.api.Test;
 
 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;
 import java.util.Random;
+import java.util.concurrent.TimeUnit;
 
 import static org.assertj.core.api.Assertions.assertThat;
 
@@ -55,7 +58,7 @@ public class ParserTest {
   void runTidyParser() throws MotionGrammarParser.ParseException {
 
     // for some reason, the best random seed value here is 1 and not 0???
-    World world = RobotScene.initialWorld(new Random(1));
+    RobotScene world = RobotScene.initialWorld(new Random(1));
 
     // create a parser using the world
     RobotParser parser = new RobotParser(world);
@@ -72,4 +75,55 @@ public class ParserTest {
     }
   }
 
+  @Test
+  @Disabled
+  void runRealTidyParser() throws MotionGrammarParser.ParseException {
+
+    MotionGrammarConfig.astDiagramDir = TIDY_AST_DIAGRAM_DIR;
+
+    RobotScene world = RobotScene.initialWorld();
+
+    org.fusesource.mqtt.client.MQTT handler = new org.fusesource.mqtt.client.MQTT();
+    try {
+      handler.setHost("tcp://localhost:1883");
+    } catch (URISyntaxException e) {
+      e.printStackTrace();
+    }
+    world.connection = handler.blockingConnection();
+    try {
+      world.connection.connect();
+    } catch (Exception e) {
+      e.printStackTrace();
+    }
+
+    System.err.println(world.connection.isConnected());
+
+    try {
+      world.connectTable("mqtt://localhost//ceti_cell_empty/scene/update");
+    } catch (IOException e) {
+      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);
+
+    // parse (synchonously, long-running)
+    var result = parser.parse();
+
+    assertThat(result).isNotNull().isInstanceOf(Tidy.class);
+    try {
+      Thread.sleep(100000*1000);
+    } catch (InterruptedException e) {
+      e.printStackTrace();
+    }
+  }
+
 }