continue testing for multi-robot

ros3rag.placeB/build.gradle

@@ -27,7 +27,7 @@ ext.sharedJastAddDir = 'src/main/jastadd/shared'
 ext.ragConnectInputGrammar = 'src/main/jastadd/WorldModelB.relast'
 ext.ragConnectInputConnect = 'src/main/jastadd/WorldModelB.connect'
 ext.ragConnectRootNode = 'WorldModelB'
-ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/main/jastadd/BFS/BFS.relast", "src/gen/jastadd/RagConnect.relast"]
+ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/main/jastadd/BFS/BFS.relast", "src/main/jastadd/RobotReachabilityToBFS.relast", "src/gen/jastadd/RagConnect.relast"]
 ext.jastaddAstPackage = 'de.tudresden.inf.st.placeB.ast'
 
 apply from: '../ros3rag.common/src/main/resources/tasks.gradle'

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

 import java.util.*;
 
 aspect BFS {
-  syn List<Vertex> Vertex.BFS(Vertex goal) {
-    Map<Vertex, Vertex> pred = new HashMap<>();
+  syn List<Edge> Vertex.BFS(Vertex goal) {
+    Map<Vertex, Edge> pred = new HashMap<>();
     Queue<Vertex> Q = new LinkedList<>();
     Set<Vertex> seen = new HashSet<>();
     seen.add(this);
@@ -18,34 +18,47 @@ aspect BFS {
           continue;
         }
         seen.add(w);
-        pred.put(w, v);
+        pred.put(w, e);
         Q.add(w);
       }
-      // uncomment following for-loop if edges are bidirectional
       for (Edge e : v.getIncomingList()) {
+        if (!e.getBidirectional()) { continue; }
         Vertex w = e.getFrom();
         if (seen.contains(w)) {
           continue;
         }
         seen.add(w);
-        pred.put(w, v);
+        pred.put(w, e);
         Q.add(w);
       }
     }
     // failure
-    return Collections.emptyList();
+    return null;
   }
 
-  private List<Vertex> Vertex.reconstructFrom(Vertex goal, Map<Vertex, Vertex> pred) {
-    List<Vertex> result = new ArrayList<>();
+  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)) {
-      result.add(current);
-      current = pred.get(current);
+      Edge e = pred.get(current);
+      result.add(e);
+      current = e.getFrom().equals(current) ? e.getTo() : e.getFrom();
     }
-    result.add(this);
+//    result.add(this);
     Collections.reverse(result);
     return result;
   }
+
+  public String Graph.prettyPrint() {
+    StringJoiner sj = new StringJoiner("\n", "Graph {\n", "\n}");
+    if (!hasVertex()) { sj.add("<no vertices>"); }
+    for (Vertex v : getVertexList()) { sj.add(v.toString()); }
+    if (!hasEdge()) { sj.add("<no edges>"); }
+    for (Edge e : getEdgeList()) { sj.add(e.prettyPrint()); }
+    return sj.toString();
+  }
+  public String Edge.prettyPrint() {
+    return this.toString() + " : " + getFrom() + (getBidirectional() ? " <-> " : " -> ") + getTo();
+  }
 }

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

 Graph ::= Vertex* Edge* ;
 Vertex ;
-Edge ;
+Edge ::= <Bidirectional:boolean> ;
 rel Edge.From <-> Vertex.Outgoing* ;
 rel Edge.To <-> Vertex.Incoming* ;

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

@@ -7,6 +7,7 @@ aspect RobotReachabilityToBFS {
     Map<LogicalDropOffLocation, Vertex> mapping = new HashMap<>();
     for (LogicalDropOffLocation loc : getMyScene().getLogicalScene().getLogicalDropOffLocationList()) {
       Vertex vertex = new Vertex();
+      vertex.setLocation(loc);
       result.addVertex(vertex);
       mapping.put(loc, vertex);
     }
@@ -18,6 +19,8 @@ aspect RobotReachabilityToBFS {
         for (LogicalObjectOfInterest other : reachableLocations) {
           if (obj == other) { continue; }
           Edge edge = new Edge();
+          edge.setRobot(robot);
+          edge.setBidirectional(true);
           edge.setFrom(mapping.get(obj));
           edge.setTo(mapping.get(other));
           result.addEdge(edge);
@@ -26,4 +29,25 @@ aspect RobotReachabilityToBFS {
     }
     return result;
   }
+
+  syn Optional<Vertex> LogicalDropOffLocation.correspondingVertex() {
+    if (!worldModelB().hasMyScene()) {
+      return Optional.empty();
+    }
+    for (Vertex v : worldModelB().toReachabilityGraph().getVertexList()) {
+      if (v.getLocation() == this) {
+        return Optional.of(v);
+      }
+    }
+    return Optional.empty();
+  }
+}
+
+aspect Printint {
+  public String Vertex.toString() {
+    return "V(" + getLocation().getName() + ")";
+  }
+  public String Edge.toString() {
+    return "E[" + getRobot().getName() + "]";
+  }
 }

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

@@ -163,7 +163,7 @@ aspect Computation {
     }
     List<LogicalObjectOfInterest> result = new ArrayList<>();
     for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
-      worldModelB().getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(canReachObj.getObjectName());
+      result.add(worldModelB().getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(canReachObj.getObjectName()));
     }
     return result;
   }

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

@@ -13,7 +13,9 @@ import java.nio.charset.StandardCharsets;
 import java.nio.file.Files;
 import java.nio.file.Path;
 import java.nio.file.Paths;
+import java.util.List;
 import java.util.concurrent.TimeUnit;
+import java.util.stream.Collectors;
 
 import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
 import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
@@ -37,8 +39,54 @@ public class SimpleMainB {
 //    testReceivingModelB();
 //    testReachability();
 //    testWeirdBehaviour();
-//    printReachability();
-    testBFS();
+//    testReachability();
+//    testBFS();
+    testRobotReachabilityBFS();
+  }
+
+  private void testRobotReachabilityBFS() throws Exception {
+    WorldModelB model = new WorldModelB();
+    de.tudresden.inf.st.ceti.Scene scene = readScene(
+        UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json")
+    );
+    Scene myScene = UtilB.convert(scene);
+    model.setMyScene(myScene);
+    for (String name : Util.extractRobotNames(scene)) {
+      Robot robot = UtilB.createRobot(name);
+      model.addRobot(robot);
+      String filenameReachability = "src/main/resources/dummy-reachability-b-" + name + ".json";
+      Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(filenameReachability));
+      Reachability reachability = UtilB.readReachability(path);
+      CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
+      robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
+    }
+
+    printReachability(model);
+    printShortestPath(model, "binRed", "binGreen");
+    printShortestPath(model, "binBlue", "binYellow");
+    printShortestPath(model, "binRed", "binPurple");
+  }
+
+  private void printReachability(WorldModelB model) {
+    System.out.println("ModelInfos:");
+    System.out.println(UtilB.getModelInfos(model, true));
+
+    System.out.println("Reachability:");
+    model.getRobotList().forEach(r -> System.out.println(r.getName() + ": " + r.reachableObjects().stream().map(LogicalObjectOfInterest::getName).collect(Collectors.joining(", "))));
+
+    System.out.println("ReachabilityGraph:");
+    System.out.println(model.toReachabilityGraph().prettyPrint());
+  }
+
+  private void printShortestPath(WorldModelB model, String source, String target) {
+    LogicalDropOffLocation sourceLocation = model.getMyScene().getLogicalScene()
+        .resolveLogicalObjectOfInterest(source).asLogicalDropOffLocation();
+    LogicalDropOffLocation targetLocation = model.getMyScene().getLogicalScene()
+        .resolveLogicalObjectOfInterest(target).asLogicalDropOffLocation();
+
+    List<Edge> bfs = sourceLocation.correspondingVertex().orElseThrow().BFS(targetLocation.correspondingVertex().orElseThrow());
+
+    System.out.println("Shortest path from " + sourceLocation.getName() + " to " + targetLocation.getName() + ": " + bfs);
   }
 
   private void testBFS() {
@@ -84,7 +132,7 @@ public class SimpleMainB {
     return result;
   }
 
-  private void printReachability() throws Exception {
+  private void testDistance() throws Exception {
     de.tudresden.inf.st.ceti.Scene scene = readScene(
         UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json")
     );
@@ -165,7 +213,7 @@ public class SimpleMainB {
     model.ragconnectCloseConnections();
   }
 
-  private void testReachability() throws Exception {
+  private void testDummyReachability() throws Exception {
     WorldModelB model = new WorldModelB();
     Robot arm1 = UtilB.createRobot("ARM1");
     model.addRobot(arm1);

ros3rag.placeB/src/main/resources/config-b.yaml

@@ -6,3 +6,5 @@ reachability:
     filename: "src/main/resources/dummy-reachability-b-arm1.json"
   - idRobot: "arm2"
     filename: "src/main/resources/dummy-reachability-b-arm2.json"
+  - idRobot: "arm3"
+    filename: "src/main/resources/dummy-reachability-b-arm3.json"

ros3rag.placeB/src/main/resources/config-scene-b.json

@@ -7,6 +7,8 @@
   { "id": "binBlue","type": "DROP_OFF_LOCATION","pos": { "x": 1.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
   { "id": "binRed","type": "DROP_OFF_LOCATION","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
   { "id": "binGreen","type": "DROP_OFF_LOCATION","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
+  { "id": "binYellow","type": "DROP_OFF_LOCATION","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } },
+  { "id": "binPurple","type": "DROP_OFF_LOCATION","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } },
   { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
   { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
   { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
@@ -17,5 +19,6 @@
   { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
   { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
   { "id": "arm1","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } },
-  { "id": "arm2","type": "ARM","pos": { "x": 1, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
+  { "id": "arm2","type": "ARM","pos": { "x": 1, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } },
+  { "id": "arm3","type": "ARM","pos": { "x": 2, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
 ] }

ros3rag.placeB/src/main/resources/dummy-reachability-b-arm3.json

+{
+  "idRobot": "ARM3",
+  "objects": [
+    { "idObject": "objectRed1", "reachable": true },
+    { "idObject": "objectRed2", "reachable": true },
+    { "idObject": "objectRed3", "reachable": false },
+    { "idObject": "binRed", "reachable": false },
+    { "idObject": "binBlue", "reachable": false },
+    { "idObject": "binGreen", "reachable": true },
+    { "idObject": "binYellow", "reachable": false },
+    { "idObject": "binPurple", "reachable": true }
+  ]
+}