using and testing BFS algorithm to compute operations

ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag

@@ -38,7 +38,6 @@ aspect BFS {
 
   private List<Edge> Vertex.reconstructFrom(Vertex goal, Map<Vertex, Edge> pred) {
     List<Edge> result = new ArrayList<>();
-    System.out.println(pred);
     Vertex current = goal;
     while (!current.equals(this)) {
       Edge e = pred.get(current);

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

@@ -62,59 +62,22 @@ aspect Computation {
   //--- computeOperations ---
   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()) {
-      // check if robot can reach both, location (if any) and the object
-      if ((!hasPreviousLocation() || robot.canReach(getPreviousLocation().getName())) &&
-           robot.canReach(getObject().getName())) {
-        robotsFitForPreviousLocation.add(robot);
-      }
-      if (robot.canReach(getNewLocation().getName())) {
-        robotsFitForNewLocation.add(robot);
-      }
-    }
-    if (robotsFitForPreviousLocation.isEmpty()) {
-      StringBuilder sb = new StringBuilder("No robot can reach object ");
-      sb.append(getObject().getName());
-      if (hasPreviousLocation()) {
-        sb.append(" and ").append(getPreviousLocation().getName());
-      }
-      sb.append("!");
-      return Collections.singletonList(new ErrorOperation(sb.toString()));
-    }
-    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.getCanReachObjectOfInterestWrapper().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 getPreviousLocation().correspondingVertex().map(previousVertex -> {
+      return getNewLocation().correspondingVertex().map(newVertex -> {
+        List<Edge> shortestPath = previousVertex.BFS(newVertex);
+        if (shortestPath == null || shortestPath.isEmpty()) {
+          return error("No sequence of operations to move " + getObject().getName() + (hasPreviousLocation() ? " from " + getPreviousLocation().getName() : "") + " to " + getNewLocation().getName());
+        }
+        List<Operation> result = new ArrayList<>();
+        Vertex transit = previousVertex;
+        for (Edge edge : shortestPath) {
+          Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
+          result.add(createPickAndPlace(edge.getRobot(), getObject(), target.getLocation()));
+          transit = target;
+        }
         return result;
+      }).orElseGet(() -> error("Could not resolve graph vertex of previous location " + getNewLocation().getName()));
+    }).orElseGet(() -> error("Could not resolve graph vertex of new location " + getPreviousLocation().getName()));
   }
   eq DifferenceNewObject.computeOperations() {
     // FIXME. stub, may be implemented later
@@ -125,6 +88,11 @@ aspect Computation {
     return Collections.emptyList();
   }
 
+  //--- error ---
+  protected static List<Operation> Difference.error(String message) {
+    return Collections.singletonList(new ErrorOperation(message));
+  }
+
   //--- createPickAndPlace ---
   private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) {
     var result = new PickAndPlace();

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java

 package de.tudresden.inf.st.placeB;
 
-import com.google.protobuf.util.JsonFormat;
 import de.tudresden.inf.st.ceti.Reachability;
 import de.tudresden.inf.st.placeB.ast.*;
 import de.tudresden.inf.st.ros3rag.common.Configuration;
@@ -9,8 +8,6 @@ import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
 import java.io.IOException;
-import java.nio.charset.StandardCharsets;
-import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
 import java.util.List;

ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java

+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+import org.junit.jupiter.api.Test;
+
+import java.util.List;
+
+import static org.assertj.core.api.Assertions.assertThat;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+/**
+ * Testing computing operations from differences.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestComputeOperations {
+  @Test
+  public void testNoMovePossible() {
+    WorldModelB model = TestUtils.newModel();
+    TestUtils.addObjects(model, "x");
+    TestUtils.addLocations(model, "red", "blue", "green");
+    TestUtils.addRobots(model, "arm1", "arm2");
+    TestUtils.addReachability(model, "arm1", "red", "blue");
+    TestUtils.addReachability(model, "arm2", "blue");
+
+    LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+    LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+    LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+    LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+    DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green);
+
+    List<Operation> operations = diff.computeOperations();
+    assertThat(operations).isNotEmpty();
+    assertThat(operations).hasSize(1);
+    Operation op = operations.get(0);
+    assertTrue(op.isErrorOperation());
+    assertEquals("+Error: No sequence of operations to move x from red to green+", op.prettyPrint());
+  }
+
+  @Test
+  public void testDirectMove() {
+    WorldModelB model = TestUtils.newModel();
+    TestUtils.addObjects(model, "x");
+    TestUtils.addLocations(model, "red", "blue", "green");
+    TestUtils.addRobots(model, "arm1", "arm2");
+    TestUtils.addReachability(model, "arm1", "red", "blue");
+    TestUtils.addReachability(model, "arm2", "blue");
+
+    LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+    LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+    LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+    LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+    Robot arm1 = model.findRobot("arm1").orElseThrow();
+    DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, blue);
+
+    List<Operation> operations = diff.computeOperations();
+    assertThat(operations).isNotEmpty();
+    assertThat(operations).hasSize(1);
+    Operation op = operations.get(0);
+    assertThat(op).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace = (PickAndPlace) op;
+    assertEquals(arm1, pickAndPlace.getRobotToExecute());
+    assertEquals(x, pickAndPlace.getObjectToPick());
+    assertEquals(blue, pickAndPlace.getTargetLocation());
+  }
+
+  @Test
+  public void testIndirectMoveWith2Robots() {
+    WorldModelB model = TestUtils.newModel();
+    TestUtils.addObjects(model, "x");
+    TestUtils.addLocations(model, "red", "blue", "green");
+    TestUtils.addRobots(model, "arm1", "arm2");
+    TestUtils.addReachability(model, "arm1", "red", "blue");
+    TestUtils.addReachability(model, "arm2", "blue", "green");
+
+    LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+    LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+    LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+    LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+    LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+    Robot arm1 = model.findRobot("arm1").orElseThrow();
+    Robot arm2 = model.findRobot("arm2").orElseThrow();
+    DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green);
+
+    List<Operation> operations = diff.computeOperations();
+    assertThat(operations).isNotEmpty();
+    assertThat(operations).hasSize(2);
+    Operation op1 = operations.get(0);
+    assertThat(op1).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+    assertEquals(arm1, pickAndPlace1.getRobotToExecute());
+    assertEquals(x, pickAndPlace1.getObjectToPick());
+    assertEquals(blue, pickAndPlace1.getTargetLocation());
+
+    Operation op2 = operations.get(1);
+    assertThat(op2).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+    assertEquals(arm2, pickAndPlace2.getRobotToExecute());
+    assertEquals(x, pickAndPlace2.getObjectToPick());
+    assertEquals(green, pickAndPlace2.getTargetLocation());
+  }
+
+  @Test
+  public void testIndirectMoveWith3Robots() {
+    /*
+     * red -(arm1)-> blue --(arm3)-,
+     *    `-(arm2)-> green <-------'
+     *                 `--(arm4)-> yellow -(arm5)-> purple
+     */
+    WorldModelB model = TestUtils.newModel();
+    TestUtils.addObjects(model, "x");
+    TestUtils.addLocations(model, "red", "blue", "green", "yellow", "purple");
+    TestUtils.addRobots(model, "arm1", "arm2", "arm3", "arm4", "arm5");
+    TestUtils.addReachability(model, "arm1", "red", "blue");
+    TestUtils.addReachability(model, "arm2", "red", "green");
+    TestUtils.addReachability(model, "arm3", "blue", "green");
+    TestUtils.addReachability(model, "arm4", "green", "yellow");
+    TestUtils.addReachability(model, "arm5", "yellow", "purple");
+
+    LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+    LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+    LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+    LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+    LogicalDropOffLocation yellow = logicalScene.resolveLogicalObjectOfInterest("yellow").asLogicalDropOffLocation();
+    LogicalDropOffLocation purple = logicalScene.resolveLogicalObjectOfInterest("purple").asLogicalDropOffLocation();
+    Robot arm2 = model.findRobot("arm2").orElseThrow();
+    Robot arm4 = model.findRobot("arm4").orElseThrow();
+    Robot arm5 = model.findRobot("arm5").orElseThrow();
+    DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, purple);
+
+    List<Operation> operations = diff.computeOperations();
+    assertThat(operations).isNotEmpty();
+    assertThat(operations).hasSize(3);
+
+    Operation op1 = operations.get(0);
+    assertThat(op1).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+    assertEquals(arm2, pickAndPlace1.getRobotToExecute());
+    assertEquals(x, pickAndPlace1.getObjectToPick());
+    assertEquals(green, pickAndPlace1.getTargetLocation());
+
+    Operation op2 = operations.get(1);
+    assertThat(op2).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+    assertEquals(arm4, pickAndPlace2.getRobotToExecute());
+    assertEquals(x, pickAndPlace2.getObjectToPick());
+    assertEquals(yellow, pickAndPlace2.getTargetLocation());
+
+    Operation op3 = operations.get(2);
+    assertThat(op3).isInstanceOf(PickAndPlace.class);
+    PickAndPlace pickAndPlace3 = (PickAndPlace) op3;
+    assertEquals(arm5, pickAndPlace3.getRobotToExecute());
+    assertEquals(x, pickAndPlace3.getObjectToPick());
+    assertEquals(purple, pickAndPlace3.getTargetLocation());
+  }
+}

ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java

+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+
+/**
+ * Utility methods for testing.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestUtils {
+  public static WorldModelB newModel() {
+    WorldModelB model = new WorldModelB();
+    Scene scene = new Scene();
+    model.setMyScene(scene);
+    return model;
+  }
+
+  public static void addObjects(WorldModelB model, String... names) {
+    for (String name : names) {
+      int index = model.getMyScene().getNumMovableObject();
+      MovableObject obj = new MovableObject()
+          .setName(name)
+          .setPosition(new Position(-index, -index, -index))
+          .setOrientation(new Orientation())
+          .setSize(new Size(1, 1, 1));
+      model.getMyScene().addMovableObject(obj);
+    }
+  }
+
+  public static void addLocations(WorldModelB model, String... names) {
+    for (String name : names) {
+      int index = model.getMyScene().getNumDropOffLocation();
+      DropOffLocation obj = new DropOffLocation()
+          .setName(name)
+          .setPosition(new Position(+index, +index, +index))
+          .setOrientation(new Orientation())
+          .setSize(new Size(1, 1, 1));
+      model.getMyScene().addDropOffLocation(obj);
+    }
+  }
+
+  public static void addRobots(WorldModelB model, String... names) {
+    for (String name : names) {
+      Robot obj = new Robot().setName(name);
+      model.addRobot(obj);
+    }
+  }
+
+  public static void addReachability(WorldModelB model, String robotName, String... reachableObjects) {
+    Robot robot = model.findRobot(robotName).orElseThrow();
+    CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper();
+    for (String reachableObjectName : reachableObjects) {
+      wrapper.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(reachableObjectName));
+    }
+    robot.setCanReachObjectOfInterestWrapper(wrapper);
+  }
+
+}