From 97baeda61404b3c017eee1a174f0eec598092981 Mon Sep 17 00:00:00 2001 From: rschoene <rene.schoene@tu-dresden.de> Date: Thu, 28 Apr 2022 17:06:22 +0200 Subject: [PATCH] use active of robot in scene as busy-indicator of robot in model --- .../src/main/resources/jastadd/types.connect | 8 +++- .../src/main/resources/jastadd/types.jadd | 13 +++++++ .../src/main/resources/jastadd/types.relast | 8 ++-- .../main/jastadd/RobotReachabilityToBFS.jrag | 2 +- .../src/main/jastadd/WorldModelB.connect | 21 ---------- .../src/main/jastadd/WorldModelB.jadd | 33 +--------------- .../src/main/jastadd/WorldModelB.relast | 2 +- .../tudresden/inf/st/placeB/SimpleMainB.java | 38 ++++++++++++++----- .../de/tudresden/inf/st/placeB/UtilB.java | 2 +- 9 files changed, 57 insertions(+), 70 deletions(-) diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect index dd61bb5..28bc80d 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.connect +++ b/ros3rag.common/src/main/resources/jastadd/types.connect @@ -25,7 +25,13 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {: result.addDropOffLocation(cz); obj = cz; break; - // TODO maybe add ARM here as well? + case ARM: + // ARM == RobotObject + RobotObject ro = new RobotObject(); + ro.setActive(pbObject.getActive()); + result.addRobotObject(ro); + obj = ro; + break; default: // ignore object, continue for loop continue; diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd index 2d80774..91f383e 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -15,6 +15,11 @@ aspect Resolving { return movableObject; } } + for (RobotObject robotObject : getRobotObjectList()) { + if (robotObject.getName().equals(name)) { + return robotObject; + } + } return null; } @@ -135,6 +140,14 @@ aspect Navigation { syn DropOffLocation ObjectOfInterest.asDropOffLocation() = null; eq DropOffLocation.asDropOffLocation() = this; + // --- isRobotObject --- + syn boolean ObjectOfInterest.isRobotObject() = false; + eq RobotObject.isRobotObject() = true; + + // --- asRobotObject --- + syn RobotObject ObjectOfInterest.asRobotObject() = null; + eq RobotObject.asRobotObject() = this; + // --- isLogicalRegion --- syn boolean LogicalObjectOfInterest.isLogicalRegion() = false; eq LogicalRegion.isLogicalRegion() = true; diff --git a/ros3rag.common/src/main/resources/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast index c31c1c3..8061ea5 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.relast +++ b/ros3rag.common/src/main/resources/jastadd/types.relast @@ -3,14 +3,14 @@ Size ::= <Length:double> <Width:double> <Height:double> ; Orientation ::= <X:double> <Y:double> <Z:double> <W:double> ; // Regions cannot be contained in scene, but must be retrieved via attribute -Scene ::= DropOffLocation* MovableObject* /LogicalScene/ ; +Scene ::= DropOffLocation* MovableObject* RobotObject* /LogicalScene/ ; CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ; ObjectOfInterest ::= <Name:String> Position Size Orientation ; -DropOffLocation : ObjectOfInterest ::= ; - -MovableObject : ObjectOfInterest ::= ; +DropOffLocation : ObjectOfInterest ; +MovableObject : ObjectOfInterest ; +RobotObject : ObjectOfInterest ::= <Active:boolean> ; CanReachObjectOfInterest ::= <ObjectName:String> ; diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag index 93adc36..97a95cb 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag @@ -20,7 +20,7 @@ aspect RobotReachabilityToBFS { // mapping.put(obj, vertex); // } for (Robot robot : getRobotList()) { - if (robot.getBusy()) { + if (robot.isBusy()) { continue; } List<DropOffLocation> reachableLocations = robot.reachableObjects().stream() diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect index 379efaa..2ef0239 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect @@ -4,7 +4,6 @@ receive indexed WorldModelB.OtherScene ; receive WorldModelB.TestingOtherScene ; receive Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ; receive Robot.OwnedCollaborationZoneNames using ConfigChangeCommandCheckForOwnedCollaborationZone ; -receive Robot.Busy using ConfigChangeCommandCheckForBusy ; // --- sending --- send WorldModelB.NextOperation using PrintOperation ; @@ -50,23 +49,3 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {: } return String.join(",", collaborationZoneNames); :} - -ConfigChangeCommandCheckForBusy maps byte[] bytes to boolean {: - de.tudresden.inf.st.ceti.Command command = null; - try { - command = de.tudresden.inf.st.ceti.Command.parseFrom(bytes); - } catch (com.google.protobuf.InvalidProtocolBufferException e) { - reject(); - } - System.out.println(command); - // if the command is not a PickAndPlace command, do not change anything - if (!command.hasPickAndPlace()) { - reject(); - } - Robot thisRobot = (Robot) this; - // if the command is not about this robot, do not change anything - if (!thisRobot.getName().equals(command.getPickAndPlace().getIdRobot())) { - reject(); - } - return true; -:} diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 217dc18..3d7f79e 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -272,39 +272,8 @@ aspect Computation { } 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 { -// syn List<ObjectOfInterest> Robot.getReachableObjectOfInterestList() { -// List<ObjectOfInterest> result = new ArrayList<>(); -// for (CanReachObjectOfInterest ref : getCanReachObjectOfInterestList()) { -// result.add(resolveObjectOfInterest(ref.getObjectName())); -// } -// return result; -// } -//} - aspect AttributeMappings { //--- toProtobufByteArray --- syn byte[] Operation.toProtobufByteArray() = null; @@ -380,6 +349,8 @@ aspect Navigation { } return result; } + + syn boolean Robot.isBusy() = worldModelB().getMyScene().resolveObjectOfInterest(getName()).asRobotObject().getActive(); } aspect GlueForShared { diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast index 856d1b0..e6bb98b 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast @@ -1,7 +1,7 @@ WorldModelB ::= Region* Robot* [MyScene:Scene] OtherScene:LogicalScene* /NextOperation:Operation/ TestingOtherScene:LogicalScene* ; // FIXME inline CanReachObjectOfInterestWrapper -Robot ::= <Name:String> CanReachObjectOfInterestWrapper <Busy:boolean> <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneName> ; +Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneName> ; // relations into nodes received by RagConnect are not allowed //rel Robot.OwnedCollaborationZone* <-> CollaborationZone.Owner? ; //rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ; 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 9b8602b..281accc 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 @@ -18,6 +18,7 @@ import java.util.Arrays; import java.util.List; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; +import java.util.function.Function; import java.util.stream.Collectors; /** @@ -112,7 +113,6 @@ public class SimpleMainB { model.connectOtherScene(Util.mqttUri(topicUpdateFromPlaceA, config), 0); model.connectMyScene(Util.mqttUri(topicSceneUpdateB, config)); for (Robot robot : model.getRobotList()) { - robot.connectBusy(Util.mqttUri(topicCommand, config)); robot.connectOwnedCollaborationZoneNames(Util.mqttUri(topicCommand, config)); } model.connectNextOperation(Util.mqttUri(topicCommand, config), true); @@ -161,7 +161,7 @@ public class SimpleMainB { mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); describedWait(4, "set R1 to not busy"); - model.findRobot("R1").ifPresentOrElse(robot -> robot.setBusy(false), () -> logger.warn("Did not find R1")); + scene = updateNotBusyOfRobot(scene, "R1"); describedWait(5, "print model status"); mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); @@ -175,7 +175,8 @@ public class SimpleMainB { mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); describedWait(8, "set R2 to not busy"); - model.findRobot("R2").ifPresentOrElse(robot -> robot.setBusy(false), () -> logger.warn("Did not find R2")); + scene = updateNotBusyOfRobot(scene, "R2"); + mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); describedWait(9, "print model status"); mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); @@ -198,18 +199,34 @@ public class SimpleMainB { de.tudresden.inf.st.ceti.Scene scene, String objectName, Position newPosition) { + return updateObject(scene, objectName, obj -> { + Object.Builder builder = obj.toBuilder(); + builder.getPosBuilder() + .setX(newPosition.getX()) + .setY(newPosition.getY()) + .setZ(newPosition.getZ()); + return builder.build(); + }); + } + + static de.tudresden.inf.st.ceti.Scene updateNotBusyOfRobot( + de.tudresden.inf.st.ceti.Scene scene, + String objectName) { + return updateObject(scene, objectName, obj -> obj.toBuilder().setActive(false).build()); + } + + static de.tudresden.inf.st.ceti.Scene updateObject( + de.tudresden.inf.st.ceti.Scene scene, + String objectName, + Function<Object, Object> change + ) { List<Object> objectsList = scene.getObjectsList(); Object newObj = null; int index, objectsListSize; for (index = 0, objectsListSize = objectsList.size(); index < objectsListSize; index++) { Object obj = objectsList.get(index); if (obj.getId().equals(objectName)) { - Object.Builder builder = obj.toBuilder(); - builder.getPosBuilder() - .setX(newPosition.getX()) - .setY(newPosition.getY()) - .setZ(newPosition.getZ()); - newObj = builder.build(); + newObj = change.apply(obj); break; } } @@ -217,11 +234,12 @@ public class SimpleMainB { logger.error("Did not find object {}!", objectName); } else { scene = scene.toBuilder().setObjects(index, newObj).build(); - logger.info("Update {} in scene to {}", objectName, newObj); + logger.info("Update {} in scene to:\n {}", objectName, newObj); } return scene; } + // copied from WorldModelA protected static byte[] _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(LogicalScene input) throws Exception { java.io.ByteArrayOutputStream outputStream = new java.io.ByteArrayOutputStream(); 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 8ad4ec6..d58dc9e 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 @@ -99,7 +99,7 @@ public class UtilB { sb.append("myRobots: ") .append(model.getRobotList().prettyPrint( robot -> robot.getName() - + "(" + (robot.getBusy() ? "busy" : "free") + + "(" + (robot.isBusy() ? "busy" : "free") + ", canReach: " + robot.getCanReachObjectOfInterestWrapper() .getCanReachObjectOfInterestList() -- GitLab