From 0d84a0cd4cb48991e543805e19b9faae2ea79086 Mon Sep 17 00:00:00 2001 From: rschoene <rene.schoene@tu-dresden.de> Date: Wed, 15 Jun 2022 16:27:09 +0200 Subject: [PATCH] update - generalize demo commands - avoid sending duplicate commands for same robot - direct manipulation in mappings is dangerous (tested, added only for documentation) - use NTAs for error operations --- ...ros3rag.java-ragconnect-conventions.gradle | 3 +- .../src/main/jastadd/WorldModelB.connect | 19 +++ .../src/main/jastadd/WorldModelB.jadd | 116 +++++++++++++++--- .../de/tudresden/inf/st/placeB/MainB.java | 81 ++++++------ 4 files changed, 159 insertions(+), 60 deletions(-) diff --git a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle index 9a79e8a..f4c8f22 100644 --- a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle +++ b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle @@ -15,6 +15,5 @@ dependencies { relast group: 'org.jastadd', name: 'relast', version: "0.4.0-143" ragconnect group: 'de.tudresden.inf.st', name: 'ragconnect', version: '1.0.0-alpha-203' - implementation group: 'de.tudresden.inf.st', name: 'dumpAst', version: '1.0.2-68' + implementation group: 'de.tudresden.inf.st', name: 'dumpAst', version: '1.0.3-69' } - diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect index da2da8d..302541d 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect @@ -46,7 +46,9 @@ ConvertCommand maps de.tudresden.inf.st.ceti.Command command to Operation {: // --- sending --- send WorldModelB.NextOperation using PrintOperation ; +// (direct manipulation is dangerous) send WorldModelB.NextOperation using PrintAndRememberOperation ; send Robot.myPosition(String) ; +// (direct manipulation is dangerous) send Robot.myPosition(String) using ImmediateUpdate ; PrintOperation maps Operation op to byte[] {: byte[] result = op.toProtobufByteArray(); @@ -56,6 +58,23 @@ PrintOperation maps Operation op to byte[] {: return result; :} +// (direct manipulation is dangerous) +//PrintAndRememberOperation maps Operation op to byte[] {: +// byte[] result = op.toProtobufByteArray(); +// if (result == null) { +// reject(); +// } +// ((WorldModelB) this).addExecutedOperation(op); +// return result; +//:} +// +// (direct manipulation is dangerous) +//ImmediateUpdate maps String pos to String {: +// Robot robot = (Robot) this; +// robot.setCurrentPosition(pos); +// return pos; +//:} + ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {: de.tudresden.inf.st.ceti.Command command = null; try { diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 704964c..2c5a092 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -276,17 +276,37 @@ aspect Computation { return new Wait(); } + syn Operation WorldModelB.lastOperationFor(Robot robot) { + if (robot == null) { + return null; + } + for (int index = getNumExecutedOperation() - 1; index >= 0; index--) { + Operation op = getExecutedOperation(index); + if (robot.equals(op.getRobotToExecute())) { + return op; + } + } + return null; + } + + syn nta ErrorOperation WorldModelB.errorNoOperationComputed() = new ErrorOperation().setErrorMessage("No operation computed!"); + syn nta ErrorOperation WorldModelB.errorDuplicateOperation() = new ErrorOperation().setErrorMessage("Do not send duplicate operation twice!"); + syn nta ErrorOperation WorldModelB.errorNoExecutableOperation() = new ErrorOperation().setErrorMessage("No executable operation found!"); //--- getNextOperation --- syn Operation WorldModelB.getNextOperation() { if (diffToOperations().getNumChild() == 0) { - return new ErrorOperation().setErrorMessage("No operation computed!"); + return errorNoOperationComputed(); } for (Operation op : diffToOperations()) { + Robot executingRobot = op.getRobotToExecute(); if (!op.isErrorOperation()) { + if (op.equals(lastOperationFor(executingRobot))) { + return errorDuplicateOperation(); + } return op; } } - return new ErrorOperation().setErrorMessage("No executable operation found!"); + return errorNoExecutableOperation(); } //--- canReach --- @@ -390,24 +410,24 @@ aspect AttributeMappings { throw new RuntimeException("Separate Place operation not supported yet!"); } eq PickAndPlace.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder() - .setPickAndPlace(de.tudresden.inf.st.ceti.PickAndPlace.newBuilder() + .setPickAndPlace(de.tudresden.inf.st.ceti.PickAndPlace.newBuilder() .setIdRobot(getRobotToExecute().getName()) .setIdPick(getObjectToPick().getName()) .setIdPlace(getTargetLocation().getName()) .build()) - .build().toByteArray(); + .build().toByteArray(); eq ConfigChange.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder() - .setConfigChange(de.tudresden.inf.st.ceti.ConfigChange.newBuilder() + .setConfigChange(de.tudresden.inf.st.ceti.ConfigChange.newBuilder() .setIdRobotNewOwner(getRobotToExecute().getName()) .setIdCollaborationZone(getCollaborationZone().getName()) .build()) - .build().toByteArray(); + .build().toByteArray(); eq Evacuate.toProtobufByteArray() = de.tudresden.inf.st.ceti.Command.newBuilder() - .setEvacuate(de.tudresden.inf.st.ceti.Evacuate.newBuilder() + .setEvacuate(de.tudresden.inf.st.ceti.Evacuate.newBuilder() .setIdRobot(getRobotToExecute().getName()) .setIdCollaborationZone(getCollaborationZone().getName()) .build()) - .build().toByteArray(); + .build().toByteArray(); eq Wait.toProtobufByteArray() = null; } @@ -427,11 +447,45 @@ aspect Navigation { syn boolean Operation.isErrorOperation() = false; eq ErrorOperation.isErrorOperation() = true; - syn Region LogicalRegion.realRegion() = worldModelB().findRegion(getName()); + syn boolean Operation.isPickAndPlace() = false; + eq PickAndPlace.isPickAndPlace() = true; + + syn boolean Operation.isConfigChange() = false; + eq ConfigChange.isConfigChange() = true; + + syn boolean Operation.isEvacuate() = false; + eq Evacuate.isEvacuate() = true; + + syn ErrorOperation Operation.asErrorOperation() = null; + eq ErrorOperation.asErrorOperation() = this; + + syn PickAndPlace Operation.asPickAndPlace() = null; + eq PickAndPlace.asPickAndPlace() = this; + + syn ConfigChange Operation.asConfigChange() = null; + eq ConfigChange.asConfigChange() = this; + + syn Evacuate Operation.asEvacuate() = null; + eq Evacuate.asEvacuate() = this; + + syn Region LogicalRegion.realRegion() { + if (hasWorldModelB()) { + return worldModelB().findRegion(getName()); + } + return new Region().setName("unknown region for " + getName()); + } + + private boolean LogicalRegion.hasWorldModelB() { + ASTNode parent = getParent(); + while (parent.getParent() != null) { + parent = parent.getParent(); + } + return parent instanceof WorldModelB; + } //--- allMappings --- -// inh LocationMapping LogicalDropOffLocation.allMappings(); -// eq WorldModelB.getChild().allMappings() = getLocationMapping(); + // inh LocationMapping LogicalDropOffLocation.allMappings(); + // eq WorldModelB.getChild().allMappings() = getLocationMapping(); syn boolean CollaborationZone.hasOccupient() = occupient() != null; syn Robot CollaborationZone.occupient() { // (>) == moving in, (^) == evacuate/moving out @@ -458,7 +512,9 @@ aspect Navigation { syn List<String> Robot.occupiedCollaborationZonesAsList() = arrayAsList(getOccupiedCollaborationZoneNames()); eq Region.locationList() { List<DropOffLocation> result = new ArrayList<>(); - if (!worldModelB().hasMyScene()) { return result; } + if (!worldModelB().hasMyScene()) { + return result; + } for (String locationName : locationNamesAsList()) { result.add(worldModelB().getMyScene().resolveObjectOfInterest(locationName).asDropOffLocation()); } @@ -485,7 +541,8 @@ aspect GlueForShared { inh LogicalObjectOfInterest Difference.resolveLogicalObjectOfInterest(String name); eq WorldModelB.diffScenes().resolveLogicalObjectOfInterest(String name) = getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(name); - class WorldModelB implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {} + class WorldModelB implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper { + } eq WorldModelB.getMyScene().regionList() = getRegionList(); } @@ -543,6 +600,35 @@ aspect DumpAst { } } +aspect Comparison { + boolean Operation.equals(Operation other) { return false; } + boolean PickAndPlace.equals(Operation other) { + if (other == null || !other.isPickAndPlace()) { + return false; + } + PickAndPlace otherPickAndPlace = other.asPickAndPlace(); + return getRobotToExecute().equals(otherPickAndPlace.getRobotToExecute()) && + getObjectToPick().getName().equals(otherPickAndPlace.getObjectToPick().getName()) && + getTargetLocation().getName().equals(otherPickAndPlace.getTargetLocation().getName()); + } + boolean ConfigChange.equals(Operation other) { + if (other == null || !other.isConfigChange()) { + return false; + } + ConfigChange otherConfigChange = other.asConfigChange(); + return getRobotToExecute().equals(otherConfigChange.getRobotToExecute()) && + getCollaborationZone().getName().equals(otherConfigChange.getCollaborationZone().getName()); + } + boolean Evacuate.equals(Operation other) { + if (other == null || !other.isEvacuate()) { + return false; + } + Evacuate otherEvacuate = other.asEvacuate(); + return getRobotToExecute().equals(otherEvacuate.getRobotToExecute()) && + getCollaborationZone().getName().equals(otherEvacuate.getCollaborationZone().getName()); + } +} + aspect Resolving { //--- findRobot --- public Optional<Robot> WorldModelB.findRobot(String name) { @@ -564,8 +650,8 @@ aspect Resolving { return null; } -// // do not resolve real regions and real locations (send by site-A) -// refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null; + // // do not resolve real regions and real locations (send by site-A) + // refine RefResolverStubs eq LogicalMovableObject.resolveMyLocationByToken(String name) = null; } aspect RagConnectAddOn { public void WorldModelB.ragconnectResetEvaluationCounter() { diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java index 7e3a199..9df4a09 100644 --- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java @@ -44,13 +44,20 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected void createSpecificMainHandlerConnections() { mainHandler.newConnection("place-b/demo", bytes -> { - String topic = new String(bytes); + String command = new String(bytes); + int colonIndex = command.indexOf(":"); + if (colonIndex == -1) { + logger.error("Unknown demo command {}", command); + return; + } + String key = command.substring(0, colonIndex); + String value = command.substring(colonIndex + 1); int slashIndex; - switch (topic) { - case "objectRed1/red": - UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed"); - break; - case "initial_scene": + switch (key) { + case "scene": + if (!value.equals("initial")) { + logger.warn("Can only send initial scene, but got {}. Sending initial scene.", command); + } try { demo_scene = Util.readScene(UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-placeworld-manual.json")); mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray()); @@ -58,33 +65,17 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { e.printStackTrace(); } break; - case "arm1/idle": - case "arm1/picking": - case "arm1/placing": - case "arm1/moving": - case "arm2/idle": - case "arm2/picking": - case "arm2/placing": - case "arm2/moving": - slashIndex = topic.indexOf("/"); - String robot = topic.substring(0, slashIndex); - String stateString = "STATE_" + topic.substring(slashIndex + 1).toUpperCase(); + case "robot": + slashIndex = value.indexOf("/"); + String robot = value.substring(0, slashIndex); + String stateString = "STATE_" + value.substring(slashIndex + 1).toUpperCase(); Object.State state = Object.State.valueOf(stateString); updateAndPublishScene(robot, r -> r.toBuilder().setState(state).build()); break; - case "bigBlue/cz1": - case "bigBlue/G1": - case "bigBlue/G2": - case "bigBlue/I1": - case "bigBlue/I2": - case "bigGreen/cz1": - case "bigGreen/G1": - case "bigGreen/G2": - case "bigGreen/I1": - case "bigGreen/I2": - slashIndex = topic.indexOf("/"); - String obj = topic.substring(0, slashIndex); - String location = topic.substring(slashIndex + 1); + case "object": + slashIndex = value.indexOf("/"); + String obj = value.substring(0, slashIndex); + String location = value.substring(slashIndex + 1); Position pos = model.getMyScene().resolveObjectOfInterest(location).asDropOffLocation().getPosition(); updateAndPublishScene(obj, o -> { Object.Builder builder = o.toBuilder(); @@ -96,7 +87,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { }); break; default: - logger.error("Unknown demo command {}", topic); + logger.error("Unknown demo command {}", command); } }); } @@ -171,18 +162,22 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected String getModelInfos(WorldModelB worldModelB, boolean detailed) { - try { - String filename = model.dumpAst(builder -> { - builder.excludeChildren("Orientation", "Size"); - builder.excludeRelations("ContainedInRegion"); - builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations"); - builder.includeAttributes("realRegion", "computeOperations", "myPosition"); - builder.includeNullNodes(); - }); - mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8)); - } catch (Exception e) { - logger.catching(e); - } +// Thread t = new Thread(() -> { + try { + String filename = model.dumpAst(builder -> { + builder.excludeChildren("Orientation", "Size"); + builder.excludeRelations("ContainedInRegion"); + builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations"); + builder.includeAttributes("realRegion", "computeOperations", "myPosition"); + builder.includeNullNodes(); + }); + mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8)); + } catch (Exception e) { + logger.catching(e); + } +// }); +// t.setDaemon(true); +// t.start(); return UtilB.getModelInfos(model, detailed); } -- GitLab