Multiple scenes, multiple robots and more

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<>();
+    Queue<Vertex> Q = new LinkedList<>();
+    Set<Vertex> seen = new HashSet<>();
+    seen.add(this);
+    Q.add(this);
+    while (!Q.isEmpty()) {
+      Vertex v = Q.remove();
+      if (v.equals(goal)) {
+        return reconstructFrom(goal, pred);
+      }
+      for (Edge e : v.getOutgoingList()) {
+        Vertex w = e.getTo();
+        if (seen.contains(w)) {
+          continue;
+        }
+        seen.add(w);
+        pred.put(w, v);
+        Q.add(w);
+      }
+      // uncomment following for-loop if edges are bidirectional
+      for (Edge e : v.getIncomingList()) {
+        Vertex w = e.getFrom();
+        if (seen.contains(w)) {
+          continue;
+        }
+        seen.add(w);
+        pred.put(w, v);
+        Q.add(w);
+      }
+    }
+    // failure
+    return Collections.emptyList();
+  }
+
+  private List<Vertex> Vertex.reconstructFrom(Vertex goal, Map<Vertex, Vertex> pred) {
+    List<Vertex> result = new ArrayList<>();
+    System.out.println(pred);
+    Vertex current = goal;
+    while (!current.equals(this)) {
+      result.add(current);
+      current = pred.get(current);
+    }
+    result.add(this);
+    Collections.reverse(result);
+    return result;
+  }
+}