diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle index 1f16b72742b27666e4d19100b60d9eced53233a2..a92210145c1a70d9648b7d390c3dd7568e6b46dd 100644 --- a/ros3rag.placeB/build.gradle +++ b/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/gen/jastadd/RagConnect.relast"] +ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/main/jastadd/BFS/BFS.relast", "src/gen/jastadd/RagConnect.relast"] ext.jastaddAstPackage = 'de.tudresden.inf.st.placeB.ast' apply from: '../ros3rag.common/src/main/resources/tasks.gradle' diff --git a/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag new file mode 100644 index 0000000000000000000000000000000000000000..281801e3d6987a1d8790c4bc9ff5d54f27e0bedd --- /dev/null +++ b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag @@ -0,0 +1,51 @@ +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; + } +} diff --git a/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast b/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast new file mode 100644 index 0000000000000000000000000000000000000000..9e51b2500c7a9b70638ae807fff12c67f5557f9d --- /dev/null +++ b/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast @@ -0,0 +1,5 @@ +Graph ::= Vertex* Edge* ; +Vertex ; +Edge ; +rel Edge.From <-> Vertex.Outgoing* ; +rel Edge.To <-> Vertex.Incoming* ; diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag new file mode 100644 index 0000000000000000000000000000000000000000..9a64dfa17dfbedf6d672b385231fe4c5d068f199 --- /dev/null +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag @@ -0,0 +1,29 @@ +aspect RobotReachabilityToBFS { + syn nta Graph WorldModelB.toReachabilityGraph() { + Graph result = new Graph(); + if (!hasMyScene()) { + return result; + } + Map<LogicalDropOffLocation, Vertex> mapping = new HashMap<>(); + for (LogicalDropOffLocation loc : getMyScene().getLogicalScene().getLogicalDropOffLocationList()) { + Vertex vertex = new Vertex(); + result.addVertex(vertex); + mapping.put(loc, vertex); + } + for (Robot robot : getRobotList()) { + List<LogicalObjectOfInterest> reachableLocations = robot.reachableObjects().stream() + .filter(LogicalObjectOfInterest::isLogicalDropOffLocation) + .collect(java.util.stream.Collectors.toList()); + for (LogicalObjectOfInterest obj : reachableLocations) { + for (LogicalObjectOfInterest other : reachableLocations) { + if (obj == other) { continue; } + Edge edge = new Edge(); + edge.setFrom(mapping.get(obj)); + edge.setTo(mapping.get(other)); + result.addEdge(edge); + } + } + } + return result; + } +} diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast new file mode 100644 index 0000000000000000000000000000000000000000..914e7b4ff9f5ee02cea3de883edb390705de083e --- /dev/null +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast @@ -0,0 +1,2 @@ +rel Vertex.Location -> LogicalDropOffLocation ; +rel Edge.Robot -> Robot ; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 636fce9c0febed3679de6b37893dc97b08269124..6d5fdc0a10f0210f287f8a043b0e04a2fe0606ec 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -156,6 +156,17 @@ aspect Computation { } return false; } + + syn List<LogicalObjectOfInterest> Robot.reachableObjects() { + if (!worldModelB().hasMyScene()) { + return Collections.emptyList(); + } + List<LogicalObjectOfInterest> result = new ArrayList<>(); + for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) { + worldModelB().getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(canReachObj.getObjectName()); + } + return result; + } } //aspect RelationsByReference { diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java index 5b9c8e0cd38e208ca303cd35054bfb1aaf32820e..88ac7ccebd8975952e5880fdbdf9e1d72789d85c 100644 --- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java @@ -37,7 +37,51 @@ public class SimpleMainB { // testReceivingModelB(); // testReachability(); // testWeirdBehaviour(); - printReachability(); +// printReachability(); + testBFS(); + } + + private void testBFS() { + /* + A B + | \ | + C - D + */ + Graph g = new Graph(); + Vertex a = makeVertex("a"); + Vertex b = makeVertex("b"); + Vertex c = makeVertex("c"); + Vertex d = makeVertex("d"); + Edge ac = makeEdge(a, c); + Edge ad = makeEdge(a, d); + Edge cd = makeEdge(c, d); + Edge db = makeEdge(d, b); + g.addVertex(a); + g.addVertex(b); + g.addVertex(c); + g.addVertex(d); + g.addEdge(ac); + g.addEdge(ad); + g.addEdge(cd); + g.addEdge(db); + + System.out.println(a.BFS(b)); + } + + private Vertex makeVertex(String name) { + return new Vertex() { + @Override + public String toString() { + return name; + } + }; + } + + private Edge makeEdge(Vertex from, Vertex to) { + Edge result = new Edge(); + result.setFrom(from); + result.setTo(to); + return result; } private void printReachability() throws Exception {