diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd index 0d9fe88eb118fec13f1aadec7f72440fbefab760..b2f7f85f1e0826e1fde6c9865571204108f3be69 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -20,9 +20,9 @@ aspect Resolving { //--- resolveLogicalObjectOfInterest --- syn LogicalObjectOfInterest LogicalScene.resolveLogicalObjectOfInterest(String name) { - for (LogicalDropOffLocation location : getLogicalDropOffLocationList()) { - if (location.getName().equals(name)) { - return location; + for (LogicalRegion region : getLogicalRegionList()) { + if (region.getName().equals(name)) { + return region; } } for (LogicalMovableObject movableObject : getLogicalMovableObjectList()) { @@ -32,6 +32,15 @@ aspect Resolving { } return null; } + +// syn LocationMappingPair LocationMapping.findMappingFor(LogicalDropOffLocation location) { +// for (LocationMappingPair pair : getLocationMappingPairList()) { +// if (pair.getLogicalLocation().equals(location)) { +// return pair; +// } +// } +// return null; +// } } aspect Computation { @@ -74,6 +83,15 @@ aspect Computation { LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight); } + syn MovableObject DropOffLocation.getObjectLocatedHere() { + for (MovableObject obj : containingScene().getMovableObjects()) { + if (obj.isLocatedAt(this)) { + return obj; + } + } + return null; + } + //--- getLogicalScene --- syn LogicalScene Scene.getLogicalScene() { var result = new LogicalScene(); @@ -84,13 +102,15 @@ aspect Computation { result.addLogicalMovableObject(newLogicalMovableObject); objects.put(movableObject, newLogicalMovableObject); } - for (DropOffLocation location : getDropOffLocationList()) { - var logicalLocation = new LogicalDropOffLocation(); - logicalLocation.setName(location.getName()); - result.addLogicalDropOffLocation(logicalLocation); + for (Region region : getRegionList()) { + var logicalRegion = new LogicalRegion(); + logicalRegion.setName(region.getName()); + result.addLogicalRegion(logicalRegion); for (MovableObject movableObject : getMovableObjectList()) { - if (movableObject.isLocatedAt(location)) { - logicalLocation.addContainedObject(objects.get(movableObject)); + for (DropOffLocation location : region.getLocationList()) { + if (movableObject.isLocatedAt(location)) { + logicalRegion.addContainedObject(objects.get(movableObject)); + } } } } @@ -109,13 +129,21 @@ aspect Computation { } aspect Navigation { - // --- isLogicalDropOffLocation --- - syn boolean LogicalObjectOfInterest.isLogicalDropOffLocation() = false; - eq LogicalDropOffLocation.isLogicalDropOffLocation() = true; + // --- isDropOffLocation --- + syn boolean ObjectOfInterest.isDropOffLocation() = false; + eq DropOffLocation.isDropOffLocation() = true; + + // --- asDropOffLocation --- + syn DropOffLocation ObjectOfInterest.asDropOffLocation() = null; + eq DropOffLocation.asDropOffLocation() = this; - // --- asLogicalDropOffLocation --- - syn LogicalDropOffLocation LogicalObjectOfInterest.asLogicalDropOffLocation() = null; - eq LogicalDropOffLocation.asLogicalDropOffLocation() = this; + // --- isLogicalRegion --- + syn boolean LogicalObjectOfInterest.isLogicalRegion() = false; + eq LogicalRegion.isLogicalRegion() = true; + + // --- asLogicalRegion --- + syn LogicalRegion LogicalObjectOfInterest.asLogicalRegion() = null; + eq LogicalRegion.asLogicalRegion() = this; // --- isLogicalMovableObject --- syn boolean LogicalObjectOfInterest.isLogicalMovableObject() = false; @@ -124,6 +152,21 @@ aspect Navigation { // --- asLogicalMovableObject --- syn LogicalMovableObject LogicalObjectOfInterest.asLogicalMovableObject() = null; eq LogicalMovableObject.asLogicalMovableObject() = this; + + // --- isCollaborationZone --- + syn boolean DropOffLocation.isCollaborationZone() = false; + eq CollaborationZone.isCollaborationZone() = true; + + // --- asCollaborationZone --- + syn CollaborationZone DropOffLocation.asCollaborationZone() = null; + eq CollaborationZone.asCollaborationZone() = this; + + inh Scene DropOffLocation.containingScene(); + inh Scene MovableObject.containingScene(); + eq Scene.getDropOffLocation().containingScene() = this; + eq Scene.getMovableObject().containingScene() = this; + + syn boolean LogicalMovableObject.hasLocatedAt() = !getLocatedAtList().isEmpty(); } aspect Printing { @@ -155,11 +198,15 @@ aspect Printing { } syn String MovableObject.prettyPrint() { - return "<obj " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "@" + Integer.toHexString(hashCode()) + ">"; + return "<obj " + nameAndHash() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + ">"; } syn String DropOffLocation.prettyPrint() { - return "{loc " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "@" + Integer.toHexString(hashCode()) + "}"; + return "{loc " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "}"; + } + + syn String Region.prettyPrint() { + return "{reg " + nameAndHash() + "}"; } syn String Position.prettyPrint() { @@ -176,13 +223,14 @@ aspect Printing { syn String LogicalScene.prettyPrint() { StringBuilder sb = new StringBuilder(); - if (getNumLogicalDropOffLocation() == 0) { - sb.append(" no locations\n"); + if (getNumLogicalRegion() == 0) { + sb.append(" no region\n"); } else { - for (var location : getLogicalDropOffLocationList()) { - sb.append(" location ").append(location.prettyPrint()); - if (!location.getContainedObjectList().isEmpty()) { - sb.append(" (objects: ").append(location.getContainedObjectList()).append(")"); + // TODO + for (LogicalRegion region : getLogicalRegionList()) { + sb.append(" region ").append(region.prettyPrint()); + if (!region.getContainedObjectList().isEmpty()) { + sb.append(" (objects: ").append(region.getContainedObjectList().stream().map(LogicalMovableObject::getName).collect(java.util.stream.Collectors.joining(","))).append(")"); } sb.append("\n"); } @@ -190,10 +238,10 @@ aspect Printing { if (getNumLogicalMovableObject() == 0) { sb.append(" no objects\n"); } else { - for (var obj : getLogicalMovableObjectList()) { + for (LogicalMovableObject obj : getLogicalMovableObjectList()) { sb.append(" obj ").append(obj.prettyPrint()); if (obj.hasLocatedAt()) { - sb.append(" (locatedAt: ").append(obj.getLocatedAt()).append(")"); + sb.append(" (locatedAt: ").append(obj.getLocatedAtList().stream().map(LogicalRegion::nameAndHash).collect(java.util.stream.Collectors.joining(","))).append(")"); } sb.append("\n"); } @@ -202,11 +250,15 @@ aspect Printing { } syn String LogicalMovableObject.prettyPrint() { - return "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">"; + return "<lObj " + nameAndHash() + ">"; } - syn String LogicalDropOffLocation.prettyPrint() { - return "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}"; + syn String LogicalRegion.prettyPrint() { + return "{lReg " + nameAndHash() + "}"; } + + syn String LogicalObjectOfInterest.nameAndHash() = getName() + "@" + Integer.toHexString(hashCode()); + syn String ObjectOfInterest.nameAndHash() = getName() + "@" + Integer.toHexString(hashCode()); + syn String Region.nameAndHash() = getName() + "@" + Integer.toHexString(hashCode()); } aspect ConvenienceMethods { diff --git a/ros3rag.common/src/main/resources/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast index 8e2c8badd07a4c3fbb5b305db8df9b4b2294df3a..4948abe610da2f9e743d1ad4f7339678a96bf89e 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.relast +++ b/ros3rag.common/src/main/resources/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* /LogicalScene/ ; +Scene ::= Region* DropOffLocation* MovableObject* /LogicalScene/ ; CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ; ObjectOfInterest ::= <Name:String> Position Size Orientation ; @@ -13,18 +13,14 @@ MovableObject : ObjectOfInterest ::= ; CanReachObjectOfInterest ::= <ObjectName:String> ; -LogicalScene ::= LogicalDropOffLocation* LogicalMovableObject* ; +LogicalScene ::= LogicalRegion* LogicalMovableObject* ; LogicalObjectOfInterest ::= <Name:String> ; -LogicalDropOffLocation : LogicalObjectOfInterest ; +LogicalRegion : LogicalObjectOfInterest ; LogicalMovableObject : LogicalObjectOfInterest ; -rel LogicalDropOffLocation.ContainedObject* <-> LogicalMovableObject.LocatedAt? ; -// rs: assumption movable object can only be at one location at the same time, i.e., no overlapping locations +rel LogicalRegion.ContainedObject* <-> LogicalMovableObject.LocatedAt* ; -// new as of 2022-04-21 -LocationMapping ::= LocationMappingPair* ; -LocationMappingPair ; -rel LocationMappingPair.Location -> LogicalDropOffLocation ; -rel LocationMappingPair.Position* -> Position ; // TODO what is the target here? is it really Position? +Region ::= <Name:String> ; +rel Region.Location* <-> DropOffLocation.ContainedInRegion* ; -// should this be only in site-B?? -CollaborationZone : LogicalDropOffLocation ; +// TODO should this be only in site-B?? +CollaborationZone : DropOffLocation ; diff --git a/ros3rag.placeA/src/main/resources/config-scene-a-2022.json b/ros3rag.placeA/src/main/resources/config-scene-a-2022.json new file mode 100644 index 0000000000000000000000000000000000000000..909502501d1839ae8767587e003376e584f7daba --- /dev/null +++ b/ros3rag.placeA/src/main/resources/config-scene-a-2022.json @@ -0,0 +1,21 @@ +{ "objects": [ + { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "P1","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } }, + { "id": "P2","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "P3","type": "DROP_OFF_LOCATION","pos": { "x": 2,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } }, + { "id": "P4","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } }, + { "id": "P5","type": "DROP_OFF_LOCATION","pos": { "x": 2,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P6","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P-E","type": "DROP_OFF_LOCATION","pos": { "x": 5,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P-F","type": "DROP_OFF_LOCATION","pos": { "x": 5,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "O1","type": "BOX","pos": { "x": 8,"y": 4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "O2","type": "BOX","pos": { "x": 2,"y": 2, "z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "O3","type": "BOX","pos": { "x": 8, "y": 2, "z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "O4-uninvolved","type": "BOX","pos": { "x": 1, "y": 4,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "O5-uninvolved","type": "BOX","pos": { "x": 1, "y": 3,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "O6-uninvolved","type": "BOX","pos": { "x": 7, "y": 3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683, "w": 0.92388 },"color": { "g": 1 } } +] } diff --git a/ros3rag.placeA/src/main/resources/mappping-a.json b/ros3rag.placeA/src/main/resources/mappping-a.json new file mode 100644 index 0000000000000000000000000000000000000000..062593023d070db663d263b0ae71e9b2df6f8f20 --- /dev/null +++ b/ros3rag.placeA/src/main/resources/mappping-a.json @@ -0,0 +1,12 @@ +{ + "mappings": [ + { "location": "L1", "positions": ["P1"] }, + { "location": "L2", "positions": ["P2"] }, + { "location": "L3", "positions": ["P3"] }, + { "location": "L4", "positions": ["P4"] }, + { "location": "L5", "positions": ["P5"] }, + { "location": "L6", "positions": ["P6"] }, + { "location": "L-E", "positions": ["P-E"] }, + { "location": "L-F", "positions": ["P-F"] } + ] +} diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle index 0df9901804825aed51c43f9ed284a383c4b03e38..5a2da9d4f49dc04cf2096201b826400e2e32dd21 100644 --- a/ros3rag.placeB/build.gradle +++ b/ros3rag.placeB/build.gradle @@ -17,6 +17,7 @@ task simpleRun(type: JavaExec) { group 'application' classpath sourceSets.main.runtimeClasspath main = "de.tudresden.inf.st.placeB.SimpleMainB" + standardInput = System.in } dependencies { diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag index cced6d479cb6ec7f922f67a059c895923ea9d2e3..a65379dedd4a92702d9608b573f4b7bbfc86c213 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag @@ -5,31 +5,36 @@ aspect RobotReachabilityToBFS { if (!hasMyScene()) { return result; } - Map<LogicalObjectOfInterest, Vertex> mapping = new HashMap<>(); - for (LogicalDropOffLocation loc : getMyScene().getLogicalScene().getLogicalDropOffLocationList()) { + Map<ObjectOfInterest, Vertex> mapping = new HashMap<>(); + // TODO insert vertices for objects, if they are contained in a location. or remove objects altogether? + for (DropOffLocation loc : getMyScene().getDropOffLocationList()) { Vertex vertex = new Vertex(); vertex.setObjectOfInterest(loc); result.addVertex(vertex); mapping.put(loc, vertex); } - for (LogicalMovableObject obj : getMyScene().getLogicalScene().getLogicalMovableObjectList()) { - Vertex vertex = new Vertex(); - vertex.setObjectOfInterest(obj); - result.addVertex(vertex); - mapping.put(obj, vertex); - } +// for (LogicalMovableObject obj : getMyScene().getLogicalScene().getLogicalMovableObjectList()) { +// Vertex vertex = new Vertex(); +// vertex.setObjectOfInterest(obj); +// result.addVertex(vertex); +// mapping.put(obj, vertex); +// } for (Robot robot : getRobotList()) { - List<LogicalObjectOfInterest> reachableLocations = robot.reachableObjects().stream() - .filter(LogicalObjectOfInterest::isLogicalDropOffLocation) + List<DropOffLocation> reachableLocations = robot.reachableObjects().stream() + .filter(ObjectOfInterest::isDropOffLocation) + .map(ObjectOfInterest::asDropOffLocation) .collect(java.util.stream.Collectors.toList()); - // bidirectional edges between locations, unidirectional edges from all reachable movableobjects to locations - for (LogicalObjectOfInterest obj : robot.reachableObjects()) { - for (LogicalObjectOfInterest other : reachableLocations) { - if (obj == other) { continue; } + // bidirectional edges between locations by default + // unidirectional edges from all occupied locations to free locations + // no edge if both locations are occupied + for (DropOffLocation loc : reachableLocations) { + for (DropOffLocation other : reachableLocations) { + if (loc == other) { continue; } + if (loc.getObjectLocatedHere() != null && other.getObjectLocatedHere() != null) { continue; } Edge edge = new Edge(); edge.setRobot(robot); - edge.setBidirectional(obj.isLogicalDropOffLocation()); - edge.setFrom(mapping.get(obj)); + edge.setBidirectional(loc.getObjectLocatedHere() == null); + edge.setFrom(mapping.get(loc)); edge.setTo(mapping.get(other)); result.addEdge(edge); } diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast index 62d0b4071475bea2960b6aed0becbad54aa9eaaf..a73376e73e68958585834372a2f12c4282b943e4 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast @@ -1,2 +1,2 @@ -rel Vertex.ObjectOfInterest -> LogicalObjectOfInterest ; +rel Vertex.ObjectOfInterest -> ObjectOfInterest ; rel Edge.Robot -> Robot ; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index e88ebe7d8c4eacc0d4e435410edac13c76dd0451..c302e0c922adf29bb0fe5ca2bef5c292fd53d438 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -1,6 +1,6 @@ aspect Computation { - //--- unspecifiedLocation --- - syn nta LogicalDropOffLocation WorldModelB.unspecifiedLocation() = new LogicalDropOffLocation().setName("<unspecified>"); + //--- unspecifiedRegion --- + syn nta LogicalRegion WorldModelB.unspecifiedRegion() = new LogicalRegion().setName("<unspecified>"); //--- mergedOtherScene --- syn nta LogicalScene WorldModelB.mergedOtherScene() { @@ -13,17 +13,17 @@ aspect Computation { return getOtherScene(0); } var result = new LogicalScene(); - // use getOtherSceneList() and merge by name all locations, objects and relations into one new, unified scene - Map<String, LogicalDropOffLocation> mergedLocations = new HashMap<>(); + // use getOtherSceneList() and merge by name all regions, objects and relations into one new, unified scene + Map<String, LogicalRegion> mergedRegions = new HashMap<>(); Map<String, LogicalMovableObject> mergedObjects = new HashMap<>(); - // first create all locations + // first create all regions for (LogicalScene scene : getOtherSceneList()) { - for (LogicalDropOffLocation loc : scene.getLogicalDropOffLocationList()) { - if (!mergedLocations.containsKey(loc.getName())) { - LogicalDropOffLocation copyOfLoc = new LogicalDropOffLocation(); - copyOfLoc.setName(loc.getName()); - mergedLocations.put(loc.getName(), copyOfLoc); - result.addLogicalDropOffLocation(copyOfLoc); + for (LogicalRegion region : scene.getLogicalRegionList()) { + if (!mergedRegions.containsKey(region.getName())) { + LogicalRegion copyOfRegion = new LogicalRegion(); + copyOfRegion.setName(region.getName()); + mergedRegions.put(region.getName(), copyOfRegion); + result.addLogicalRegion(copyOfRegion); } } } @@ -32,23 +32,23 @@ aspect Computation { for (LogicalMovableObject obj : scene.getLogicalMovableObjectList()) { if (mergedObjects.containsKey(obj.getName())) { // object already known - LogicalMovableObject copyOfObj = mergedObjects.get(obj.getName()); - if (obj.hasLocatedAt()) { - String locationName = obj.getLocatedAt().getName(); - LogicalDropOffLocation copyOfLoc = mergedLocations.get(locationName); - if (copyOfObj.hasLocatedAt() && !obj.getLocatedAt().getName().equals(copyOfObj.getLocatedAt().getName())) { - System.err.println("Overriding located-at of " + obj.getName() + " while merging (" + locationName + "," + copyOfObj.getLocatedAt().getName() + ")"); + LogicalMovableObject knownObj = mergedObjects.get(obj.getName()); + for (LogicalRegion region : obj.getLocatedAtList()) { + LogicalRegion knownRegionOfObj = mergedRegions.get(region.getName()); + if (!knownObj.getLocatedAtList().contains(knownRegionOfObj)) { + // obj has region that is not yet add to knownObj + knownObj.addLocatedAt(knownRegionOfObj); } - copyOfObj.setLocatedAt(copyOfLoc); } } else { - LogicalMovableObject copyOfObj = new LogicalMovableObject(); - copyOfObj.setName(obj.getName()); - if (obj.hasLocatedAt()) { - copyOfObj.setLocatedAt(mergedLocations.get(obj.getLocatedAt().getName())); + // object not known, needs to be created + LogicalMovableObject newObj = new LogicalMovableObject(); + newObj.setName(obj.getName()); + for (LogicalRegion region : obj.getLocatedAtList()) { + newObj.addLocatedAt(mergedRegions.get(region.getName())); } - mergedObjects.put(obj.getName(), copyOfObj); - result.addLogicalMovableObject(copyOfObj); + mergedObjects.put(obj.getName(), newObj); + result.addLogicalMovableObject(newObj); } } } @@ -78,25 +78,37 @@ aspect Computation { } LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject(); if (myObject.hasLocatedAt() && !otherObject.hasLocatedAt()) { + if (myObject.getLocatedAtList().size() > 1) { + System.err.println("More than one region found for my " + myObject.getName() + ", using only first."); + } var diff = new DifferenceObjectMisplaced(); diff.setObject(myObject); - diff.setPreviousLocation(myObject.getLocatedAt()); + // use first region + diff.setPreviousRegion(myObject.getLocatedAtList().get(0)); result.add(diff); continue; } + if (myObject.getLocatedAtList().size() > 1) { + System.err.println("More than one region found for my " + myObject.getName() + ", using only first."); + } + if (otherObject.getLocatedAtList().size() > 1) { + System.err.println("More than one region found for other " + otherObject.getName() + ", using only first."); + } + Set<LogicalRegion> myRegions = new HashSet<>(myObject.getLocatedAtList()); + Set<LogicalRegion> otherRegions = new HashSet<>(otherObject.getLocatedAtList()); if (!myObject.hasLocatedAt() && !otherObject.hasLocatedAt() || (myObject.hasLocatedAt() && otherObject.hasLocatedAt() && - myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName()))) { - // no diff if otherObject has no location information, and if both objects are located at same location + myRegions.equals(otherRegions))) { + // no diff if: otherObject has no region information, or if both objects are located at same region(s) // otherObject is always non-null, so this case does not need to be treated continue; } var difference = new DifferenceObjectAtWrongPlace(); difference.setObject(myObject); if (myObject.hasLocatedAt()) { - difference.setPreviousLocation(myObject.getLocatedAt()); + difference.setPreviousRegion(myObject.getLocatedAtList().get(0)); } - difference.setNewLocation(otherObject.getLocatedAt()); + difference.setNewRegion(otherObject.getLocatedAtList().get(0)); result.add(difference); } return result; @@ -116,22 +128,41 @@ aspect Computation { //--- computeOperations --- syn List<Operation> Difference.computeOperations(); eq DifferenceObjectAtWrongPlace.computeOperations() { - return (hasPreviousLocation() ? getPreviousLocation() : getObject()).correspondingVertex().map(previousVertex -> { - return getNewLocation().correspondingVertex().map(newVertex -> { + return (hasPreviousRegion() ? getPreviousRegion() : getObject()).correspondingVertex().map(previousVertex -> { + return getNewRegion().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()); + return error("No sequence of operations to move " + getObject().getName() + (hasPreviousRegion() ? " from " + getPreviousRegion().getName() : "") + " to " + getNewRegion().getName()); } + // TODO insert config-change and evacuate commands 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.getObjectOfInterest().asLogicalDropOffLocation())); + DropOffLocation targetLocation = target.getObjectOfInterest().asDropOffLocation(); + Robot executingRobot = edge.getRobot(); + if (targetLocation.isCollaborationZone()) { + /* here we need to check, if the collaboration zone is + (a) claimed by the executing robot, and + (b) occupied by another robot, and + (c) free (as no movable object is contained) -- this should be reflected in the graph! + */ + + // order is important here, first add ConfigChange, then Evacuate + CollaborationZone cz = targetLocation.asCollaborationZone(); + if (cz.hasOwner() && !cz.getOwner().equals(executingRobot)) { + result.add(ConfigChange.of(executingRobot, cz)); + } + if (cz.hasOccupient() && !cz.getOccupient().equals(executingRobot)) { + result.add(Evacuate.of(cz.getOccupient())); + } + } + result.add(PickAndPlace.of(executingRobot, getObject(), targetLocation)); transit = target; } return result; - }).orElseGet(() -> error("Could not resolve graph vertex of new location " + getNewLocation().getName())); - }).orElseGet(() -> error("Could not resolve graph vertex of previous location " + getPreviousLocation().getName())); + }).orElseGet(() -> error("Could not resolve graph vertex of new region " + getNewRegion().getName())); + }).orElseGet(() -> error("Could not resolve graph vertex of previous region " + getPreviousRegion().getName())); } eq DifferenceNewObject.computeOperations() { // FIXME. stub, may be implemented later @@ -147,15 +178,28 @@ aspect Computation { return Collections.singletonList(new ErrorOperation(message)); } - //--- createPickAndPlace --- - private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) { - var result = new PickAndPlace(); + //--- Operation.of --- + public static PickAndPlace PickAndPlace.of(Robot robot, LogicalMovableObject obj, DropOffLocation target) { + PickAndPlace result = new PickAndPlace(); result.setRobotToExecute(robot); result.setObjectToPick(obj); result.setTargetLocation(target); return result; } + public static ConfigChange ConfigChange.of(Robot robot, CollaborationZone cz) { + ConfigChange result = new ConfigChange(); + result.setRobotToExecute(robot); + result.setCollaborationZone(cz); + return result; + } + + public static Evacuate Evacuate.of(Robot robot) { + Evacuate result = new Evacuate(); + result.setRobotToExecute(robot); + return result; + } + //--- getNextOperation --- syn Operation WorldModelB.getNextOperation() { if (diffToOperations().getNumChild() == 0) { @@ -180,16 +224,46 @@ aspect Computation { } //--- reachableObjects --- - syn List<LogicalObjectOfInterest> Robot.reachableObjects() { + syn List<ObjectOfInterest> Robot.reachableObjects() { if (!worldModelB().hasMyScene()) { return Collections.emptyList(); } - List<LogicalObjectOfInterest> result = new ArrayList<>(); + List<ObjectOfInterest> result = new ArrayList<>(); for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) { - result.add(worldModelB().getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(canReachObj.getObjectName())); + result.add(worldModelB().getMyScene().resolveObjectOfInterest(canReachObj.getObjectName())); } return result; } + + syn DropOffLocation Region.firstFreeDropOffLocation() { + for (DropOffLocation location : getLocationList()) { + if (location.getObjectLocatedHere() == null) { + return location; + } + } + return null; + } + +// // TO DO probably should be defined on Region +// //--- isFullyOccupied (should by in types.jadd) --- +// syn boolean LogicalDropOffLocation.isFullyOccupied() { +// LocationMappingPair mapping = allMappings().findMappingFor(this); +// List<LogicalMovableObject> candidateObjects = this.getContainedObjectList(); +// // check, if all dropOffLocations are occupied +// outer: for (DropOffLocation dropOffLocation : mapping.getLocationList()) { +// // search for an object, that is located at the dropOffLocation +// for (LogicalMovableObject object : candidateObjects) { +// if (object.isLocatedAt(dropOffLocation)) { +// // found an object, continue at outer loop +// continue outer; +// } +// } +// // no object is contained in the dropOffLocation, thus, it is free. Therefore, this is not fully occupied. +// return false; +// } +// // we did not find a single dropOffLocation, that is free. Therefore, this is fully occupied +// return true; +// } } //aspect RelationsByReference { @@ -233,8 +307,13 @@ aspect Navigation { //--- worldModelB --- inh WorldModelB ASTNode.worldModelB(); eq WorldModelB.getChild().worldModelB() = this; + syn boolean Operation.isErrorOperation() = false; eq ErrorOperation.isErrorOperation() = true; + + //--- allMappings --- +// inh LocationMapping LogicalDropOffLocation.allMappings(); +// eq WorldModelB.getChild().allMappings() = getLocationMapping(); } aspect GlueForShared { @@ -248,17 +327,18 @@ aspect GlueForShared { aspect Printing { //--- prettyPrint --- - syn String Robot.prettyPrint() = "~Robot " + getName() + "~"; + syn String Robot.prettyPrint() = "~Robot " + nameAndHash() + "~"; + syn String Robot.nameAndHash() = getName() + "@" + Integer.toHexString(hashCode()); syn String Difference.prettyPrint(); eq DifferenceObjectAtWrongPlace.prettyPrint() { - return "-DifferenceObjectAtWrongPlace of " + getObject().prettyPrint() + ": " + (hasPreviousLocation() ? getPreviousLocation().prettyPrint() : "_") + " -> " + getNewLocation().prettyPrint() + "-"; + return "-DifferenceObjectAtWrongPlace of " + getObject().prettyPrint() + ": " + (hasPreviousRegion() ? getPreviousRegion().prettyPrint() : "_") + " -> " + getNewRegion().prettyPrint() + "-"; } eq DifferenceNewObject.prettyPrint() { return "-DifferenceNewObject of " + getObject().prettyPrint() + "-"; } eq DifferenceObjectMisplaced.prettyPrint() { - return "-DifferenceObjectMisplaced of " + getObject().prettyPrint() + ": " + getPreviousLocation().prettyPrint() + "-"; + return "-DifferenceObjectMisplaced of " + getObject().prettyPrint() + ": " + getPreviousRegion().prettyPrint() + "-"; } syn String Operation.prettyPrint(); diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast index 2d26f86b34708d56bf4be2994804c408a8005d84..7e84df331c5f8f9faf56ea1cded039ed1664fa15 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast @@ -7,11 +7,11 @@ rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ; abstract Difference ; rel Difference.Object -> LogicalMovableObject ; DifferenceObjectAtWrongPlace : Difference ; -rel DifferenceObjectAtWrongPlace.PreviousLocation? -> LogicalDropOffLocation ; -rel DifferenceObjectAtWrongPlace.NewLocation -> LogicalDropOffLocation ; +rel DifferenceObjectAtWrongPlace.PreviousRegion? -> LogicalRegion ; +rel DifferenceObjectAtWrongPlace.NewRegion -> LogicalRegion ; DifferenceNewObject : Difference ; DifferenceObjectMisplaced : Difference ; -rel DifferenceObjectMisplaced.PreviousLocation -> LogicalDropOffLocation ; +rel DifferenceObjectMisplaced.PreviousRegion -> LogicalRegion ; abstract Operation ; rel Operation.RobotToExecute? -> Robot ; @@ -21,14 +21,14 @@ Pick : Operation ; rel Pick.ObjectToPick -> LogicalMovableObject ; Place : Operation ; -rel Place.TargetLocation -> LogicalDropOffLocation ; +rel Place.TargetLocation -> DropOffLocation ; PickAndPlace : Operation ; rel PickAndPlace.ObjectToPick -> LogicalMovableObject ; -rel PickAndPlace.TargetLocation -> LogicalDropOffLocation ; +rel PickAndPlace.TargetLocation -> DropOffLocation ; ConfigChange : Operation ; -rel ConfigChange.CollaborationZone -> LogicalDropOffLocation ; -// TODO should be: rel ConfigChange.CollaborationZone -> CollaborationZone ; +rel ConfigChange.CollaborationZone -> DropOffLocation ; +// TODO maybe should be: rel ConfigChange.CollaborationZone -> CollaborationZone ; Evacuate : Operation ; 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 cdf0af6afef4df5e78b9e6765e5d214be7c90eed..64ab76d7ed3e3d0366378659d5c6788948d140b3 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 @@ -3,19 +3,24 @@ package de.tudresden.inf.st.placeB; import de.tudresden.inf.st.ceti.Reachability; import de.tudresden.inf.st.placeB.ast.*; import de.tudresden.inf.st.ros3rag.common.Configuration; +import de.tudresden.inf.st.ros3rag.common.Configuration.ReachabilityConfig; import de.tudresden.inf.st.ros3rag.common.Util; 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.Path; import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.Arrays; import java.util.List; +import java.util.Scanner; +import java.util.concurrent.CountDownLatch; 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; /** * Testing features for placeB. @@ -38,12 +43,114 @@ public class SimpleMainB { // testWeirdBehaviour(); // testReachability(); // testBFS(); - testRobotReachabilityBFS(); +// testRobotReachabilityBFS(); + readModelAndReceiveFromA(); } + private void readModelAndReceiveFromA() throws Exception { + Configuration config = new Configuration(); + config.mqttHost = "localhost"; + ReachabilityConfig reachabilityR1 = new ReachabilityConfig(); + reachabilityR1.idRobot = "R1"; + reachabilityR1.filename = "src/main/resources/reachability-b-r1-2022.json"; + ReachabilityConfig reachabilityR2 = new ReachabilityConfig(); + reachabilityR2.idRobot = "R2"; + reachabilityR2.filename = "src/main/resources/reachability-b-r2-2022.json"; + config.reachability = Arrays.asList(reachabilityR1, reachabilityR2); + + final String topicUpdateFromPlaceA = "update/logical/fromA"; + final String topicCommand = "command"; + final String topicExit = "place-b/exit"; + final String topicModel = "place-b/model"; + final String topicModelStatus = "place-b/status"; + + WorldModelB model = new WorldModelB(); + model.addOtherScene(new LogicalScene()); + + MqttHandler mqttHandler = new MqttHandler().dontSendWelcomeMessage(); + mqttHandler.setHost(config.mqttHost); + mqttHandler.waitUntilReady(2, TimeUnit.SECONDS); + model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); + + // read initial scene + Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-2022.json"); + de.tudresden.inf.st.ceti.Scene scene = Util.readScene(initialSceneFile); + Scene myScene = UtilB.convert(scene); + model.setMyScene(myScene); + + // init robots (copied from MainB) + for (String name : Util.extractRobotNames(scene)) { + Robot robot = UtilB.createRobot(name); + model.addRobot(robot); + // assumption: robots do not change during runtime, so we have stable connections + } + + // init reachability (copied from MainB) + for (ReachabilityConfig reachabilityConfig : config.reachability) { + Robot robot = model.findRobot(reachabilityConfig.idRobot).orElseThrow( + () -> new IllegalArgumentException("Could not find robot with id " + reachabilityConfig.idRobot) + ); + Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename)); + Reachability reachability = UtilB.readReachability(path); + CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); + robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); + } + + Runnable close = () -> { + logger.info("Exiting ..."); + mqttHandler.close(); + model.ragconnectCloseConnections(); + }; + Runtime.getRuntime().addShutdownHook(new Thread(close)); + + model.connectOtherScene(Util.mqttUri(topicUpdateFromPlaceA, config), 0); + model.connectNextOperation(Util.mqttUri(topicCommand, config), true); + + // prepare exit condition + final CountDownLatch exitCondition = new CountDownLatch(1); + mqttHandler.newConnection(topicExit, bytes -> exitCondition.countDown()); + logger.info("Quit via " + topicExit); + + // prepare model status + mqttHandler.newConnection(topicModel, bytes -> { + String message = new String(bytes); + String content = UtilB.getModelInfos(model, message.equals("Start") || message.startsWith("detail")); + logger.info("WorldModel\n{}", content); + mqttHandler.publish(topicModelStatus, content.getBytes(StandardCharsets.UTF_8)); + }); + + // read scene from A, serialize it and send it via mqtt + Path sceneAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve("src/main/resources/config-scene-a-2022.json"); + de.tudresden.inf.st.ceti.Scene scenePlaceA = Util.readScene(sceneAFile); + Scene sceneFromPlaceA = UtilB.convert(scenePlaceA); + LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene(); + byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA); + + logger.info("Wait 2 sec, then send new logical scene"); + TimeUnit.SECONDS.sleep(2); + mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend); + + logger.info("Wait another 2 sec, then print model status"); + TimeUnit.SECONDS.sleep(2); + mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + + exitCondition.await(); + } + + // copied from WorldModelA + protected static byte[] _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(LogicalScene input) throws Exception { + java.io.ByteArrayOutputStream outputStream = new java.io.ByteArrayOutputStream(); + com.fasterxml.jackson.core.JsonFactory factory = new com.fasterxml.jackson.core.JsonFactory(); + com.fasterxml.jackson.core.JsonGenerator generator = factory.createGenerator(outputStream, com.fasterxml.jackson.core.JsonEncoding.UTF8); + input.serialize(generator); + generator.flush(); + return outputStream.toString().getBytes(); + } + + @SuppressWarnings("unused") private void testRobotReachabilityBFS() throws Exception { WorldModelB model = new WorldModelB(); - de.tudresden.inf.st.ceti.Scene scene = readScene( + de.tudresden.inf.st.ceti.Scene scene = Util.readScene( UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json") ); Scene myScene = UtilB.convert(scene); @@ -69,23 +176,24 @@ public class SimpleMainB { 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(", ")))); + model.getRobotList().forEach(r -> System.out.println(r.getName() + ": " + r.reachableObjects().stream().map(ObjectOfInterest::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(); + LogicalRegion sourceLocation = model.getMyScene().getLogicalScene() + .resolveLogicalObjectOfInterest(source).asLogicalRegion(); + LogicalRegion targetLocation = model.getMyScene().getLogicalScene() + .resolveLogicalObjectOfInterest(target).asLogicalRegion(); List<Edge> bfs = sourceLocation.correspondingVertex().orElseThrow().BFS(targetLocation.correspondingVertex().orElseThrow()); System.out.println("Shortest path from " + sourceLocation.getName() + " to " + targetLocation.getName() + ": " + bfs); } + @SuppressWarnings("unused") private void testBFS() { /* A B @@ -129,8 +237,9 @@ public class SimpleMainB { return result; } + @SuppressWarnings("unused") private void testDistance() throws Exception { - de.tudresden.inf.st.ceti.Scene scene = readScene( + de.tudresden.inf.st.ceti.Scene scene = Util.readScene( UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json") ); @@ -162,6 +271,7 @@ public class SimpleMainB { } } + @SuppressWarnings("unused") private void testWeirdBehaviour() throws IOException { WorldModelB model = new WorldModelB(); Scene scene = new Scene(); @@ -187,9 +297,9 @@ public class SimpleMainB { model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); /// Connect endpoints - model.connectMyScene(mqttUri("TOPIC_MY_SCENE_UPDATE_FROM_ROS", config)); - model.connectOtherScene(mqttUri("TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A", config)); - model.connectNextOperation(mqttUri("TOPIC_COMMANDS", config), false); + model.connectMyScene(Util.mqttUri("TOPIC_MY_SCENE_UPDATE_FROM_ROS", config)); + model.connectOtherScene(Util.mqttUri("TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A", config)); + model.connectNextOperation(Util.mqttUri("TOPIC_COMMANDS", config), false); System.out.println("before"); System.out.println(scene.prettyPrint()); @@ -210,6 +320,7 @@ public class SimpleMainB { model.ragconnectCloseConnections(); } + @SuppressWarnings("unused") private void testDummyReachability() throws Exception { WorldModelB model = new WorldModelB(); Robot arm1 = UtilB.createRobot("ARM1"); @@ -259,11 +370,11 @@ public class SimpleMainB { var otherScene = new LogicalScene(); LogicalMovableObject otherObj1 = new LogicalMovableObject().setName("obj1"); otherScene.addLogicalMovableObject(otherObj1); - otherScene.addLogicalDropOffLocation(new LogicalDropOffLocation().setName("placeAlfa")); - otherScene.addLogicalDropOffLocation(new LogicalDropOffLocation().setName("placeBeta")); - LogicalDropOffLocation otherGamma = new LogicalDropOffLocation().setName("placeGamma"); + otherScene.addLogicalRegion(new LogicalRegion().setName("placeAlfa")); + otherScene.addLogicalRegion(new LogicalRegion().setName("placeBeta")); + LogicalRegion otherGamma = new LogicalRegion().setName("placeGamma"); otherGamma.addContainedObject(otherObj1); - otherScene.addLogicalDropOffLocation(otherGamma); + otherScene.addLogicalRegion(otherGamma); model.addOtherScene(otherScene); // printing and testing diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java index 64b88e1562b3a5b6739aaefad95214e0ca3d2119..773c0018762e9b96761062ccb0570601883d237e 100644 --- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java @@ -81,7 +81,7 @@ public class UtilB { return "\"" + objName + "\""; } try { - return model.getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(objName).toString(); + return model.getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(objName).getName(); } catch (NullPointerException ignore) { return "+" + objName + "(not resolved)+"; } @@ -122,6 +122,7 @@ public class UtilB { sb.append(" (unset)\n"); } sb.append("Diff: ").append(model.diffScenes().prettyPrint(Difference::prettyPrint)).append("\n"); + sb.append("Graph: ").append(model.toReachabilityGraph().prettyPrint()).append("\n"); sb.append("Operations: ").append(model.diffToOperations().prettyPrint(Operation::prettyPrint)).append("\n"); sb.append("Next operation: ").append(model.getNextOperation().prettyPrint()).append("\n"); return sb.toString(); diff --git a/ros3rag.placeB/src/main/resources/config-scene-b-2022.json b/ros3rag.placeB/src/main/resources/config-scene-b-2022.json new file mode 100644 index 0000000000000000000000000000000000000000..a9cb38250dfd291a55c9b38b5a9a76e898db9330 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/config-scene-b-2022.json @@ -0,0 +1,23 @@ +{ "objects": [ + { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "P1","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } }, + { "id": "P2","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "P3","type": "DROP_OFF_LOCATION","pos": { "x": 2,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } }, + { "id": "P4","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } }, + { "id": "P5","type": "DROP_OFF_LOCATION","pos": { "x": 2,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P6","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P-E","type": "DROP_OFF_LOCATION","pos": { "x": 5,"y": 4,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "P-F","type": "DROP_OFF_LOCATION","pos": { "x": 5,"y": 2,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 1 } }, + { "id": "O1","type": "BOX","pos": { "x": 1,"y": 2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "O2","type": "BOX","pos": { "x": 8,"y": 3, "z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "O3","type": "BOX","pos": { "x": 2, "y": 3, "z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "O4-uninvolved","type": "BOX","pos": { "x": 1, "y": 4,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "O5-uninvolved","type": "BOX","pos": { "x": 1, "y": 3,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "O6-uninvolved","type": "BOX","pos": { "x": 7, "y": 3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683, "w": 0.92388 },"color": { "g": 1 } }, + { "id": "R1","type": "ARM","pos": { "x": 1.5, "y": 3, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }, + { "id": "R2","type": "ARM","pos": { "x": 8, "y": 3, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } } +] } diff --git a/ros3rag.placeB/src/main/resources/mappping-b.json b/ros3rag.placeB/src/main/resources/mappping-b.json new file mode 100644 index 0000000000000000000000000000000000000000..062593023d070db663d263b0ae71e9b2df6f8f20 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/mappping-b.json @@ -0,0 +1,12 @@ +{ + "mappings": [ + { "location": "L1", "positions": ["P1"] }, + { "location": "L2", "positions": ["P2"] }, + { "location": "L3", "positions": ["P3"] }, + { "location": "L4", "positions": ["P4"] }, + { "location": "L5", "positions": ["P5"] }, + { "location": "L6", "positions": ["P6"] }, + { "location": "L-E", "positions": ["P-E"] }, + { "location": "L-F", "positions": ["P-F"] } + ] +} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json b/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json new file mode 100644 index 0000000000000000000000000000000000000000..f8035b8c53da83df3f6525ae437f28a770734d52 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json @@ -0,0 +1,19 @@ +{ + "idRobot": "R1", + "objects": [ + { "idObject": "O1", "reachable": true }, + { "idObject": "O2", "reachable": false }, + { "idObject": "O3", "reachable": true }, + { "idObject": "O4-uninvolved", "reachable": true }, + { "idObject": "O5-uninvolved", "reachable": true }, + { "idObject": "O6-uninvolved", "reachable": false }, + { "idObject": "P1", "reachable": true }, + { "idObject": "P2", "reachable": false }, + { "idObject": "P3", "reachable": true }, + { "idObject": "P4", "reachable": false }, + { "idObject": "P5", "reachable": true }, + { "idObject": "P6", "reachable": false }, + { "idObject": "P-E", "reachable": true }, + { "idObject": "P-F", "reachable": true } + ] +} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json b/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json new file mode 100644 index 0000000000000000000000000000000000000000..319bfca072413d5aeb6ff03292c33bc9bf9e51ac --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json @@ -0,0 +1,19 @@ +{ + "idRobot": "R2", + "objects": [ + { "idObject": "O1", "reachable": false }, + { "idObject": "O2", "reachable": true }, + { "idObject": "O3", "reachable": false }, + { "idObject": "O4-uninvolved", "reachable": false }, + { "idObject": "O5-uninvolved", "reachable": false }, + { "idObject": "O6-uninvolved", "reachable": true }, + { "idObject": "P1", "reachable": false }, + { "idObject": "P2", "reachable": true }, + { "idObject": "P3", "reachable": false }, + { "idObject": "P4", "reachable": true }, + { "idObject": "P5", "reachable": false }, + { "idObject": "P6", "reachable": true }, + { "idObject": "P-E", "reachable": true }, + { "idObject": "P-F", "reachable": true } + ] +}