more application logic

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

@@ -57,7 +57,13 @@ 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
+// vvv from rs vvv.
+// Merged message to contain both pick and place in one
+message MergedSelection {
+    string idPick = 1; // the id corresponds to an id of the Object in a Scene to be picked
+    string idPlace = 2; // the id corresponds to an id of the Object in a Scene where the picked object shall be placed
+}
+// Reachability of objects, as reported by MoveIt
 message Reachability {
     message ObjectReachability {
         string id_object = 1; // the id of the object to reach

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: not sure whether we want to receive a complete scene. could be hard to update/merge
+//     if necessary, need a mapping. the multi-point-connection is an alternative.
+//     we need to decide for one way
 
 // rs: let's try to use a "multi-point-connection" (same input meesage, multiple recipients)
 receive tree ObjectOfInterest.Position using DeserializeObject, ExtractPositionFromObject ;
@@ -38,4 +38,4 @@ ExtractOrientationFromObject maps de.tudresden.inf.st.ceti.Object o to Orientati
 :}
 
 // --- sending ---
-send tree WorldModelA.LogicalScene ;
+send tree Scene.LogicalScene ;

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

@@ -11,33 +11,23 @@ aspect Resolving {
 }
 
 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;
-  }
+//  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;
+//  }
 }

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

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

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

@@ -6,7 +6,10 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
+import java.util.Collections;
+import java.util.HashSet;
 import java.util.Scanner;
+import java.util.stream.Collectors;
 
 /**
  * Testing features for placeA.
@@ -24,6 +27,12 @@ public class SimpleMainA {
     var modelA = new WorldModelA();
     var sceneA = new Scene();
     modelA.setScene(sceneA);
+//    var a = new HashSet<>().stream()
+//        .filter(e -> true)
+//        .collect(Collectors.toSet());
+//    Collections.singletonList(1);
+//    var o = new HashSet<>().stream().findFirst();
+//    o.ifPresentOrElse();
 
     // rotate bin2 by 90°, so width and height should effectively be swapped afterwards
     Vector3D start = new Vector3D(1, 0, 0);

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

+// --- receiving ---
+receive tree WorldModelB.OtherScene ;
+
+// --- sending ---
+send tree WorldModelB.NextOperation ;

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

+aspect Computation {
+  syn nta JastAddList<Difference> WorldModelB.diffScenes() {
+    var result = new JastAddList<Difference>();
+    var myLogicalScene = getMyScene().getLogicalScene();
+    for (LogicalMovableObject otherObject : getOtherScene().getLogicalMovableObjectList()) {
+      LogicalObjectOfInterest myGenericObject = myLogicalScene.resolveLogicalObjectOfInterest(otherObject.getName());
+      if (!myGenericObject.isLogicalMovableObject()) {
+        System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!");
+        continue;
+      }
+      LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject();
+      if (myObject == null) {
+        // new object, currently not handled
+        result.add(new DifferenceNewObject());
+        continue;
+      }
+      if (!myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName())) {
+        // object at different locations
+        var difference = new DifferenceObjectAtWrongPlace();
+        difference.setObject(myObject);
+        difference.setPreviousLocation(myObject.getLocatedAt());
+        difference.setNewLocation(otherObject.getLocatedAt());
+        result.add(difference);
+      }
+      // other difference not handled yet, e.g. object deleted
+    }
+    return result;
+  }
+
+  syn nta JastAddList<Operation> WorldModelB.diffToOperations() {
+    var result = new JastAddList<Operation>();
+    for (Difference difference : diffScenes()) {
+      for (Operation operation : difference.computeOperations()) {
+        result.add(operation);
+      }
+    }
+    return result;
+  }
+
+  syn List<Operation> Difference.computeOperations();
+  eq DifferenceObjectAtWrongPlace.computeOperations() {
+    // first, find robots that can reach the locations
+    // there does not necessarily has to be one robot that can reach both
+    Set<Robot> robotsFitForPreviousLocation = new HashSet<>();
+    Set<Robot> robotsFitForNewLocation = new HashSet<>();
+    for (var robot : worldModelB().getRobotList()) {
+      if (robot.canReach(getPreviousLocation().getName())) {
+        robotsFitForPreviousLocation.add(robot);
+      }
+      if (robot.canReach(getNewLocation().getName())) {
+        robotsFitForNewLocation.add(robot);
+      }
+    }
+    if (robotsFitForPreviousLocation.isEmpty()) {
+      return Collections.singletonList(new ErrorOperation("No robot can reach previous location " + getPreviousLocation().getName()));
+    }
+    if (robotsFitForNewLocation.isEmpty()) {
+      return Collections.singletonList(new ErrorOperation("No robot can reach new location " + getNewLocation().getName()));
+    }
+    var result = new ArrayList<Operation>();
+    var robotFitForBothLocations = robotsFitForPreviousLocation.stream()
+        .filter(robotsFitForNewLocation::contains)
+        .findFirst();
+    robotFitForBothLocations.ifPresentOrElse(robot -> {
+      // there is at least one robot that can reach both locations
+      result.add(createPickAndPlace(robot, this.getObject(), this.getNewLocation()));
+    },
+    () -> {
+      // no single robot can reach both locations
+      // find a location, that can be used as exchange point reached by two robots
+      for (var robotForPrevious : robotsFitForPreviousLocation) {
+        for (var robotForNew : robotsFitForNewLocation) {
+          for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestList()) {
+            String objectName = canReachOfRobotForPrevious.getObjectName();
+            if (resolveLogicalObjectOfInterest(objectName).isLogicalDropOffLocation() && robotForNew.canReach(objectName)) {
+              // add two operations.
+              // obj -> transitLoc by robotForPrevious
+              // obj -> targetLoc by robotForNew
+              result.add(createPickAndPlace(robotForPrevious, this.getObject(), resolveLogicalObjectOfInterest(objectName).asLogicalDropOffLocation()));
+              result.add(createPickAndPlace(robotForNew, this.getObject(), this.getNewLocation()));
+          }
+        }
+      }
+    }
+    });
+    return result;
+  }
+  eq DifferenceNewObject.computeOperations() {
+    // FIXME. stub, may be implemented later
+    return Collections.emptyList();
+  }
+
+  private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) {
+    var result = new PickAndPlace();
+    result.setRobotToExecute(robot);
+    result.setObjectToPick(obj);
+    result.setTargetLocation(target);
+    return result;
+  }
+
+  syn Operation WorldModelB.getNextOperation() {
+    return diffToOperations().getChild(0);
+  }
+
+//  syn List<String> locationIds
+
+  syn boolean Robot.canReach(String objectName) {
+    for (var canReachObj : getCanReachObjectOfInterestList()) {
+      if (canReachObj.getObjectName().equals(objectName)) {
+        return true;
+      }
+    }
+    return false;
+  }
+}
+
+//aspect RelationsByReference {
+//  syn List<ObjectOfInterest> Robot.getReachableObjectOfInterestList() {
+//    List<ObjectOfInterest> result = new ArrayList<>();
+//    for (CanReachObjectOfInterest ref : getCanReachObjectOfInterestList()) {
+//      result.add(resolveObjectOfInterest(ref.getObjectName()));
+//    }
+//    return result;
+//  }
+//}
+aspect Navigation {
+  inh WorldModelB ASTNode.worldModelB();
+  eq WorldModelB.getChild().worldModelB() = this;
+}
+
+aspect GlueForShared {
+  // uncache Difference.resolveLogicalObjectOfInterest
+  inh LogicalObjectOfInterest Difference.resolveLogicalObjectOfInterest(String name);
+  eq WorldModelB.diffScenes().resolveLogicalObjectOfInterest(String name) = getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(name);
+}

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

-WorldModelB ::= Robot* MyScene:Scene OtherScene:Scene ;
+WorldModelB ::= Robot* MyScene:Scene OtherScene:LogicalScene /NextOperation:Operation/ ;
 
 Robot ::= <Name:String> CanReachObjectOfInterest* ;
+
+abstract Difference ;
+DifferenceObjectAtWrongPlace : Difference ;
+rel DifferenceObjectAtWrongPlace.Object -> LogicalMovableObject ;
+rel DifferenceObjectAtWrongPlace.PreviousLocation -> LogicalDropOffLocation ;
+rel DifferenceObjectAtWrongPlace.NewLocation -> LogicalDropOffLocation ;
+DifferenceNewObject : Difference ;
+
+abstract Operation ;
+rel Operation.RobotToExecute? -> Robot ;
+ErrorOperation : Operation ::= <ErrorMessage:String> ;
+Pick : Operation ;
+Place : Operation ;
+rel Pick.ObjectToPick -> LogicalMovableObject ;
+rel Place.TargetLocation -> LogicalDropOffLocation ;
+PickAndPlace : Operation ;
+rel PickAndPlace.ObjectToPick -> LogicalMovableObject ;
+rel PickAndPlace.TargetLocation -> LogicalDropOffLocation ;

shared.place/jastadd/types.jadd

 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
 import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
+import java.util.*;
 
 aspect GrammarTypes {
   public static Position Position.of(int x, int y, int z) {
@@ -41,6 +42,19 @@ aspect Resolving {
     }
     return null;
   }
+  syn LogicalObjectOfInterest LogicalScene.resolveLogicalObjectOfInterest(String name) {
+    for (LogicalDropOffLocation location : getLogicalDropOffLocationList()) {
+      if (location.getName().equals(name)) {
+        return location;
+      }
+    }
+    for (LogicalMovableObject movableObject : getLogicalMovableObjectList()) {
+      if (movableObject.getName().equals(name)) {
+        return movableObject;
+      }
+    }
+    return null;
+  }
 }
 
 aspect Computation {
@@ -82,6 +96,26 @@ aspect Computation {
        LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight);
   }
 
+  syn LogicalScene Scene.getLogicalScene() {
+    var result = new LogicalScene();
+    Map<MovableObject, LogicalMovableObject> objects = new HashMap<>();
+    for (DropOffLocation location : getDropOffLocationList()) {
+      var logicalLocation = new LogicalDropOffLocation();
+      logicalLocation.setName(location.getName());
+      for (MovableObject movableObject : 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;
+  }
+
   private static final double MovableObject.DELTA = 0.0001;
 
   /**
@@ -91,3 +125,21 @@ aspect Computation {
     return org.apache.commons.math3.util.Precision.compareTo(d1, d2, DELTA) <= 0;
   }
 }
+
+aspect Navigation {
+  // --- isLogicalDropOffLocation ---
+  syn boolean LogicalObjectOfInterest.isLogicalDropOffLocation() = false;
+  eq LogicalDropOffLocation.isLogicalDropOffLocation() = true;
+
+  // --- asLogicalDropOffLocation ---
+  syn LogicalDropOffLocation LogicalObjectOfInterest.asLogicalDropOffLocation() = null;
+  eq LogicalDropOffLocation.asLogicalDropOffLocation() = this;
+
+  // --- isLogicalMovableObject ---
+  syn boolean LogicalObjectOfInterest.isLogicalMovableObject() = false;
+  eq LogicalMovableObject.isLogicalMovableObject() = true;
+
+  // --- asLogicalMovableObject ---
+  syn LogicalMovableObject LogicalObjectOfInterest.asLogicalMovableObject() = null;
+  eq LogicalMovableObject.asLogicalMovableObject() = this;
+}

shared.place/jastadd/types.relast

@@ -2,7 +2,7 @@ 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* ;
+Scene ::= DropOffLocation* MovableObject* /LogicalScene/ ;
 
 ObjectOfInterest ::= <Name:String> Position Size Orientation ;