WIP: begin with some computation.

- changing grammars according to discussion
- computing isLocatedAt (not working yet)

ros3rag.common/build.gradle

@@ -7,13 +7,15 @@ buildscript {
 
 plugins {
     id 'ros3rag.java-application-conventions'
+    id 'java-library'
 }
 
 apply plugin: 'com.google.protobuf'
 
 dependencies {
     implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_version}"
-    implementation group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0'
+    api group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0'
+    api group: 'org.apache.commons', name: 'commons-math3', version: '3.6.1'
 }
 
 protobuf {

ros3rag.common/src/main/proto/proto_cgv_connector.proto → ros3rag.common/src/main/proto/cgv_connector.proto

@@ -3,6 +3,9 @@
 
 syntax = "proto3";
 
+option java_package = "de.tudresden.inf.st.ceti";
+option java_multiple_files = true;
+
 message Object {
 
     // Position is object-center related
@@ -53,3 +56,13 @@ message Scene {
 message Selection {
     string id = 1; // the id corresponds to an id of an Object in a Scene
 }
+
+// from rs. Reachability of objects, as reported by MoveIt
+message Reachability {
+    message ObjectReachability {
+        string id_object = 1; // the id of the object to reach
+        bool reachable = 2; // whether the object can be reached
+    }
+    string id_arm = 1; // the id of the robot arm
+    repeated ObjectReachability objects = 2; // all objects reachable
+}

ros3rag.placeA/build.gradle

@@ -10,17 +10,17 @@ plugins {
     id 'ros3rag.java-ragconnect-conventions'
 }
 
-mainClassName = 'de.tudresden.inf.st.placeA.MainA'
+mainClassName = 'de.tudresden.inf.st.placeA.SimpleMainA'
 
 dependencies {
     implementation project(':ros3rag.common')
 }
 
 ext.sharedJastAddDir = 'src/main/jastadd/shared'
-ext.ragConnectInputGrammar = 'src/main/jastadd/WorldModel.relast'
-ext.ragConnectInputConnect = 'src/main/jastadd/WorldModel.connect'
-ext.ragConnectRootNode = 'WorldModel'
-ext.relastFiles = ["src/gen/jastadd/WorldModel.relast", "src/gen/jastadd/RagConnect.relast"]
+ext.ragConnectInputGrammar = 'src/main/jastadd/WorldModelA.relast'
+ext.ragConnectInputConnect = 'src/main/jastadd/WorldModelA.connect'
+ext.ragConnectRootNode = 'WorldModelA'
+ext.relastFiles = ["src/gen/jastadd/WorldModelA.relast", "src/gen/jastadd/RagConnect.relast"]
 ext.jastaddAstPackage = 'de.tudresden.inf.st.placeA.ast'
 
 apply from: '../shared.place/tasks.gradle'

ros3rag.placeA/src/main/jastadd/WorldModel.jadd

-aspect Navigation {
-
-}
-
-aspect Computation {
-  syn int WorldModel.value() = 2;
-}

ros3rag.placeA/src/main/jastadd/WorldModel.relast

-WorldModel ::= RobotArm Scene ;
-
-RobotArm ::= Link* EndEffector ;
-
-Link ::= <Name:String> CurrentPosition:Coordinate ;
-
-EndEffector : Link;

ros3rag.placeA/src/main/jastadd/WorldModelA.connect

+// --- receiving ---
+
+// rs: not sure whether we want to receive a complete scene. could be hard to update/merge
+//     if necessary, need a mapping.
+receive tree WorldModelA.Scene ;
+
+// rs: let's try to use a "multi-point-connection" (same input meesage, multiple recipients)
+receive tree ObjectOfInterest.Position using DeserializeObject, ExtractPositionFromObject ;
+receive tree ObjectOfInterest.Size using DeserializeObject, ExtractSizeFromObject ;
+receive tree ObjectOfInterest.Orientation using DeserializeObject,ExtractOrientationFromObject ;
+
+DeserializeObject maps byte[] bytes to de.tudresden.inf.st.ceti.Object {:
+  return de.tudresden.inf.st.ceti.Object.parseFrom(bytes);
+:}
+//DeserializePosition maps byte[] bytes to de.tudresden.inf.st.ceti.Object.Position {:
+//  return de.tudresden.inf.st.ceti.Object.Position.parseFrom(bytes);
+//:}
+//DeserializeSize maps byte[] bytes to de.tudresden.inf.st.ceti.Object.Size {:
+//  return de.tudresden.inf.st.ceti.Object.Size.parseFrom(bytes);
+//:}
+//DeserializeOrientation maps byte[] bytes to de.tudresden.inf.st.ceti.Object.Orientation {:
+//  return de.tudresden.inf.st.ceti.Object.Orientation.parseFrom(bytes);
+//:}
+
+ExtractPositionFromObject maps de.tudresden.inf.st.ceti.Object o to Position {:
+  de.tudresden.inf.st.ceti.Object.Position pos = o.getPos();
+  return new Position(pos.getX(), pos.getY(), pos.getZ());
+:}
+
+ExtractSizeFromObject maps de.tudresden.inf.st.ceti.Object o to Size {:
+  de.tudresden.inf.st.ceti.Object.Size size = o.getSize();
+  return new Size(size.getLength(), size.getWidth(), size.getHeight());
+:}
+
+ExtractOrientationFromObject maps de.tudresden.inf.st.ceti.Object o to Orientation {:
+  de.tudresden.inf.st.ceti.Object.Orientation orient = o.getOrientation();
+  return new Orientation(orient.getX(), orient.getY(), orient.getZ(), orient.getW());
+:}
+
+// --- sending ---
+send tree WorldModelA.LogicalScene ;

ros3rag.placeA/src/main/jastadd/WorldModelA.jadd

+import java.util.List;
+import java.util.ArrayList;
+import java.util.Map;
+import java.util.HashMap;
+
+aspect Resolving {
+  inh ObjectOfInterest Robot.resolveObjectOfInterest(String name);
+  eq WorldModelA.getRobot().resolveObjectOfInterest(String name) {
+    return getScene().resolveObjectOfInterest(name);
+  }
+}
+
+aspect Computation {
+  syn LogicalScene WorldModelA.getLogicalScene() {
+    LogicalScene result = new LogicalScene();
+    Map<MovableObject, LogicalMovableObject> objects = new HashMap<>();
+    for (DropOffLocation location : getScene().getDropOffLocationList()) {
+      var logicalLocation = new LogicalDropOffLocation();
+      logicalLocation.setName(location.getName());
+      for (MovableObject movableObject : getScene().getMovableObjectList()) {
+        if (movableObject.isLocatedAt(location)) {
+          LogicalMovableObject logicalMovableObject = objects.computeIfAbsent(movableObject, k -> {
+            var newLogicalMovableObject = new LogicalMovableObject();
+            newLogicalMovableObject.setName(k.getName());
+            return newLogicalMovableObject;
+          });
+          logicalLocation.addContainedObjects(logicalMovableObject);
+        }
+      }
+    }
+    return result;
+  }
+}
+
+aspect RelationsByReference {
+  syn List<ObjectOfInterest> Robot.getReachableObjectOfInterestList() {
+    List<ObjectOfInterest> result = new ArrayList<>();
+    for (CanReachObjectOfInterest ref : getCanReachObjectOfInterestList()) {
+      result.add(resolveObjectOfInterest(ref.getObjectName()));
+    }
+    return result;
+  }
+}

ros3rag.placeA/src/main/jastadd/WorldModelA.relast

+WorldModelA ::= Robot Scene Task* /LogicalScene/ ;
+
+Robot ::= <Name:String> CanReachObjectOfInterest* ;
+
+Task ::= ObjectToMove:MovableObject TargetLocation:DropOffLocation ;

ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java

 package de.tudresden.inf.st.placeA;
 
-import de.tudresden.inf.st.placeA.ast.Coordinate;
 import de.tudresden.inf.st.placeA.ast.MqttHandler;
-import de.tudresden.inf.st.placeA.ast.WorldModel;
+import de.tudresden.inf.st.placeA.ast.Position;
+import de.tudresden.inf.st.placeA.ast.WorldModelA;
 import de.tudresden.inf.st.ros3rag.common.DataConfiguration;
 import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
@@ -22,17 +22,17 @@ public class MainA {
 
   private static final Logger logger = LogManager.getLogger(MainA.class);
   private MqttHandler mainHandler;
-  private WorldModel model;
+  private WorldModelA model;
 
   private void run(String[] args) throws IOException, InterruptedException {
-    System.out.println("This is place A with " + Coordinate.of(1, 2, 3) + "!");
+    System.out.println("This is place A with " + Position.of(1, 2, 3) + "!");
     File configFile = new File(args.length == 0 ? "../ros3rag.common/config.yaml" : args[0]);
 
     // --- No configuration below this line ---
 
     DataConfiguration.ActualConfiguration config = Util.parseConfig(configFile);
 
-    model = new WorldModel();
+    model = new WorldModelA();
 
     logStatus("Start");
     CountDownLatch exitCondition = new CountDownLatch(1);

ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java

+package de.tudresden.inf.st.placeA;
+
+import de.tudresden.inf.st.placeA.ast.*;
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import java.util.Scanner;
+
+/**
+ * Testing features for placeA.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class SimpleMainA {
+  private static final Logger logger = LogManager.getLogger(SimpleMainA.class);
+
+  public static void main(String[] args) {
+    Rotation r = new Rotation(0, 0, 1, 0, false);
+    Vector3D v = new Vector3D(1, 2, 3);
+    r.applyTo(v);
+
+    var modelA = new WorldModelA();
+    var sceneA = new Scene();
+    modelA.setScene(sceneA);
+
+    // rotate bin2 by 90°, so width and length should effectively be swapped afterwards
+    Vector3D start = new Vector3D(1, 0, 0);
+    Vector3D end = new Vector3D(0, 1, 0);
+    Rotation rot = new Rotation(start, end);
+
+    DropOffLocation bin1 = makeLocation("Bin1",
+        makePosition(1, 1, 1),
+        makeOrientation(0, 0, 0, 1),
+        makeSize(2, 3, 4));
+    DropOffLocation bin2 = makeLocation("Bin2",
+        makePosition(1, 1, 1),
+        makeOrientation((float) rot.getQ1(), (float) rot.getQ2(), (float) rot.getQ3(), (float) rot.getQ0()),
+        makeSize(2, 3, 4));
+    MovableObject objectA = makeMovableObject("ObjectA",
+        makePosition(0, 0, 0));
+    MovableObject objectB = makeMovableObject("ObjectB",
+        makePosition(1, 1, 1));
+    sceneA.addDropOffLocation(bin1)
+        .addMovableObject(objectA)
+        .addMovableObject(objectB);
+
+    /*
+     0.0 <= x <= 2.0
+    -0.5 <= y <= 2.5
+    -1.0 <= z <= 3.0
+     */
+
+    logger.info("ObjectA locatedAt Bin1: {}", objectA.isLocatedAt(bin1));
+    logger.info("ObjectB locatedAt Bin1: {}", objectB.isLocatedAt(bin1));
+    logger.info("ObjectA locatedAt Bin2: {}", objectA.isLocatedAt(bin2));
+    logger.info("ObjectB locatedAt Bin2: {}", objectB.isLocatedAt(bin2));
+    Scanner scanner = new Scanner(System.in);
+    while (true) {
+      try {
+        System.out.print("x: ");
+        float x = scanner.nextFloat();
+        System.out.print("y: ");
+        float y = scanner.nextFloat();
+        System.out.print("z: ");
+        float z = scanner.nextFloat();
+        MovableObject object = makeMovableObject("temp", makePosition(x, y, z));
+        System.out.println("located at Bin2? : " + object.isLocatedAt(bin2));
+      } catch (Exception e) {
+        break;
+      }
+    }
+  }
+
+  private static DropOffLocation makeLocation(String name, Position position, Orientation orientation, Size size) {
+    var location = new DropOffLocation();
+    initObjectOfInterest(location, name, position, orientation, size);
+    return location;
+  }
+
+  private static MovableObject makeMovableObject(String name, Position position) {
+    var location = new MovableObject();
+    initObjectOfInterest(location, name, position,
+        makeOrientation(0, 0, 0, 0), makeSize(0, 0, 0));
+    return location;
+  }
+
+  private static void initObjectOfInterest(ObjectOfInterest object, String name, Position position, Orientation orientation, Size size) {
+    object.setName(name);
+    object.setPosition(position);
+    object.setOrientation(orientation);
+    object.setSize(size);
+  }
+
+  private static Position makePosition(float x, float y, float z) {
+    return new Position(x, y, z);
+  }
+
+  private static Orientation makeOrientation(float x, float y, float z, float w) {
+    return new Orientation(x, y, z, w);
+  }
+
+  private static Size makeSize(float length, float width, float height) {
+    return new Size(length, width, height);
+  }
+}

ros3rag.placeB/build.gradle

@@ -13,6 +13,10 @@ plugins {
 
 mainClassName = 'de.tudresden.inf.st.placeB.MainB'
 
+dependencies {
+    implementation project(':ros3rag.common')
+}
+
 ext.sharedJastAddDir = 'src/main/jastadd/shared'
 ext.ragConnectInputGrammar = 'src/main/jastadd/WorldModelB.relast'
 ext.ragConnectInputConnect = 'src/main/jastadd/WorldModelB.connect'

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

-WorldModel ::= RobotArm* MyScene:Scene OtherScene:Scene ;
+WorldModelB ::= Robot* MyScene:Scene OtherScene:Scene ;
 
-Scene ::= Bin* MovableObject* ;
-
-RobotArm ::= Link* EndEffector ;
-
-Link ::= <Name:String> CurrentPosition:Coordinate ;
-
-EndEffector : Link;
-
-Bin ::= <Name:String> CurrentPosition:Coordinate ;
-
-MovableObject ::= <Name:String> CurrentPosition:Coordinate ;
+Robot ::= <Name:String> CanReachObjectOfInterest* ;

settings.gradle

 rootProject.name = 'ros3rag'
 
 include 'ros3rag.placeA'
-//include 'ros3rag.placeB'
+include 'ros3rag.placeB'
 include 'ros3rag.common'
 
 // include 'ros3rag.senderstub'

shared.place/jastadd/types.jadd

+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+
 aspect GrammarTypes {
-  public static Coordinate Coordinate.of(int x, int y, int z) {
-    return new Coordinate(x, y, z);
+  public static Position Position.of(int x, int y, int z) {
+    return new Position(x, y, z);
   }
 
   @Override
-  public boolean Coordinate.equals(Object o) {
+  public boolean Position.equals(Object o) {
     if (this == o) return true;
     if (o == null || getClass() != o.getClass()) return false;
-    Coordinate that = (Coordinate) o;
+    Position that = (Position) o;
     return getX() == that.getX() &&
         getY() == that.getY() &&
         getZ() == that.getZ();
   }
 
   @Override
-  public int Coordinate.hashCode() {
+  public int Position.hashCode() {
     return java.util.Objects.hash(getX(), getY(), getZ());
   }
 
   @Override
-  public String Coordinate.toString() {
+  public String Position.toString() {
     return "(" + getX() + ", " + getY() + ", " + getZ() + ")";
   }
 }
+
+aspect Resolving {
+  syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) {
+    for (DropOffLocation location : getDropOffLocationList()) {
+      if (location.getName().equals(name)) {
+        return location;
+      }
+    }
+    for (MovableObject movableObject : getMovableObjectList()) {
+      if (movableObject.getName().equals(name)) {
+        return movableObject;
+      }
+    }
+    return null;
+  }
+}
+
+aspect Computation {
+  syn boolean MovableObject.isLocatedAt(DropOffLocation location) {
+    Orientation orient = location.getOrientation();
+    // true to do normalization, first parameter is scalar value
+    Rotation locationRotation = new Rotation(orient.getW(), orient.getX(), orient.getY(), orient.getZ(), true);
+
+    Position pos = this.getPosition();
+    Vector3D objectVector = new Vector3D(pos.getX(), pos.getY(), pos.getZ());
+
+    // apply rotation on this point
+    Vector3D rotatedObjectVector = locationRotation.applyInverseTo(objectVector);
+
+    // check whether coordinates are within the bounds of the given location
+    Position locationPosition = location.getPosition();
+    Size locationSize = location.getSize();
+    // we add half of the size to the position to adjust for position of the location being its center
+    double rotatedX = rotatedObjectVector.getX() + 0.5 * locationSize.getLength();
+    double rotatedY = rotatedObjectVector.getY() + 0.5 * locationSize.getWidth();
+    double rotatedZ = rotatedObjectVector.getZ() + 0.5 * locationSize.getHeight();
+
+    System.out.printf("locPos: (%f , %f , %f). rotated: (%f , %f , %f), locPost+Size: (%f , %f , %f)%n",
+      locationPosition.getX(), locationPosition.getY(), locationPosition.getZ(),
+      rotatedX, rotatedY, rotatedZ,
+      locationPosition.getX() + locationSize.getLength(),
+      locationPosition.getY() + locationSize.getWidth(),
+      locationPosition.getZ() + locationSize.getHeight());
+
+    return locationPosition.getX() <= rotatedX && rotatedX <= locationPosition.getX() + locationSize.getLength() &&
+      locationPosition.getY() <= rotatedY && rotatedY <= locationPosition.getY() + locationSize.getWidth() &&
+      locationPosition.getZ() <= rotatedZ && rotatedZ <= locationPosition.getZ() + locationSize.getHeight();
+  }
+}

shared.place/jastadd/types.relast

-Coordinate ::= <X:int> <Y:int> <Z:int> ;
+Position ::= <X:float> <Y:float> <Z:float> ;
+Size ::= <Length:float> <Width:float> <Height:float> ;
+Orientation ::= <X:float> <Y:float> <Z:float> <W:float> ;
 
 Scene ::= DropOffLocation* MovableObject* ;
-DropOffLocation ::= <Name:String> CurrentPosition:Coordinate ;
-MovableObject ::= <Name:String> CurrentPosition:Coordinate ;
+
+ObjectOfInterest ::= <Name:String> Position Size Orientation ;
+
+DropOffLocation : ObjectOfInterest ::= ;
+
+MovableObject : ObjectOfInterest ::= ;
+
+CanReachObjectOfInterest ::= <ObjectName:String> ;
+
+LogicalScene ::= LogicalDropOffLocation* LogicalMovableObject* ;
+LogicalObjectOfInterest ::= <Name:String> ;
+LogicalDropOffLocation : LogicalObjectOfInterest ;
+LogicalMovableObject : LogicalObjectOfInterest ;
+rel LogicalDropOffLocation.ContainedObjects* <-> LogicalMovableObject.LocatedAt ;
+// rs: assumption movable object only on one location at the same time, i.e., no overlapping locations