diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd
index a0290bf295e70ff220f622405709a01b18f09d1d..2d8077469bcf93f69ccef4777e6093b152c6e870 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.jadd
+++ b/ros3rag.common/src/main/resources/jastadd/types.jadd
@@ -188,6 +188,10 @@ aspect Navigation {
}
syn List<String> Region.locationNamesAsList() = arrayAsList(getLocationNames()) ;
syn List<DropOffLocation> Region.locationList();
+
+ syn Set<String> LogicalMovableObject.regionNameSet() {
+ return getLocatedAtList().stream().map(LogicalRegion::getName).collect(java.util.stream.Collectors.toSet());
+ }
}
aspect Printing {
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index c96e250e67fb1222ef352eb384c6f7525a5b163d..6370aff43ffa566609597507758b0a873f61410b 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -73,7 +73,7 @@ aspect Computation {
continue;
}
if (!myGenericObject.isLogicalMovableObject()) {
- System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!");
+ System.err.println("MovableObject " + otherObject.getName() + " is not a MovableObject in myScene. Skipping!");
continue;
}
LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject();
@@ -94,8 +94,8 @@ aspect Computation {
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());
+ Set<String> myRegions = myObject.regionNameSet();
+ Set<String> otherRegions = otherObject.regionNameSet();
if (!myObject.hasLocatedAt() && !otherObject.hasLocatedAt() ||
(myObject.hasLocatedAt() && otherObject.hasLocatedAt() &&
myRegions.equals(otherRegions))) {
@@ -189,7 +189,7 @@ aspect Computation {
}
return result;
}).orElseGet(() -> error("Could not resolve graph vertex of new region " + getNewRegion().nameAndHash()));
- }).orElseGet(() -> error("Could not find a free location in " + getNewRegion().nameAndHash()));
+ }).orElseGet(() -> error("Could not find a free location in " + getNewRegion().nameAndHash() + " for " + getObject().nameAndHash()));
}).orElseGet(() -> error("Could not resolve graph vertex of previous location " + previousLocation.nameAndHash()));
}
eq DifferenceNewObject.computeOperations() {
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 5895d962e1e5adcc07bc6e23714eb712be5e8db7..9b8602b279e67de08ce0d1b43e02ec0dab4eeeee 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
@@ -149,53 +149,77 @@ public class SimpleMainB {
LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene();
byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA);
- logger.info("Wait 2 sec, then send new logical scene");
- TimeUnit.SECONDS.sleep(2);
+ describedWait(1, "send new logical scene");
mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend);
- logger.info("Wait another 2 sec, then print model status");
- TimeUnit.SECONDS.sleep(2);
+ describedWait(2, "print model status");
mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
- logger.info("Wait another 2 sec, then send updated sceneB");
- TimeUnit.SECONDS.sleep(2);
-
// set object O1 to position of P-E in sceneB and publish it
- Position posOfPE = myScene.resolveObjectOfInterest("P-E").getPosition();
+ describedWait(3, "send updated sceneB (P-E)");
+ scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P-E").getPosition());
+ 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"));
+
+ describedWait(5, "print model status");
+ mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
+ // set object O1 to position of P2.2 in sceneB and publish it
+ describedWait(6, "send updated sceneB (P2.2)");
+ scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P2.2").getPosition());
+ mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
+
+ describedWait(7, "print model status");
+ 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"));
+
+ describedWait(9, "print model status");
+ mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
+
+ if (exitAutomatically) {
+ TimeUnit.SECONDS.sleep(5);
+ exitCondition.countDown();
+ }
+ exitCondition.await();
+ }
+
+ static void describedWait(int stepNr, String description) throws InterruptedException {
+ logger.info("({}) Wait 2 secs, then {}", stepNr, description);
+ TimeUnit.SECONDS.sleep(2);
+ logger.info(description);
+ }
+
+ static de.tudresden.inf.st.ceti.Scene updatePositionOfObject(
+ de.tudresden.inf.st.ceti.Scene scene,
+ String objectName,
+ Position newPosition) {
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("O1")) {
+ if (obj.getId().equals(objectName)) {
Object.Builder builder = obj.toBuilder();
builder.getPosBuilder()
- .setX(posOfPE.getX())
- .setY(posOfPE.getY())
- .setZ(posOfPE.getZ());
+ .setX(newPosition.getX())
+ .setY(newPosition.getY())
+ .setZ(newPosition.getZ());
newObj = builder.build();
break;
}
}
if (newObj == null) {
- logger.error("Did not find object O1!");
+ logger.error("Did not find object {}!", objectName);
} else {
scene = scene.toBuilder().setObjects(index, newObj).build();
- logger.info("New scene: {}", scene);
- mqttHandler.publish(topicSceneUpdateB, scene.toByteArray());
+ logger.info("Update {} in scene to {}", objectName, newObj);
}
-
- logger.info("Wait another 2 sec, then print model status");
- TimeUnit.SECONDS.sleep(2);
- mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8));
-
- if (exitAutomatically) {
- TimeUnit.SECONDS.sleep(5);
- exitCondition.countDown();
- }
-
- exitCondition.await();
+ return scene;
}
// copied from WorldModelA
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 622b717207b5311ec5a8e267d27e502b53d8f844..8ad4ec64df1789f4bddc3c96299a3cc7bc2221c4 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
@@ -84,7 +84,7 @@ public class UtilB {
return "\"" + objName + "\"";
}
try {
- return model.getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(objName).getName();
+ return model.getMyScene().resolveObjectOfInterest(objName).getName();
} catch (NullPointerException ignore) {
return "+" + objName + "(not resolved)+";
}
@@ -98,10 +98,13 @@ public class UtilB {
StringBuilder sb = new StringBuilder();
sb.append("myRobots: ")
.append(model.getRobotList().prettyPrint(
- robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper()
+ robot -> robot.getName()
+ + "(" + (robot.getBusy() ? "busy" : "free")
+ + ", canReach: "
+ + robot.getCanReachObjectOfInterestWrapper()
.getCanReachObjectOfInterestList()
.prettyPrint(canReachObj -> resolveObjName(model, canReachObj.getObjectName()))
- + ")"))
+ + ")"))
.append("\n");
if (detailed) {
// also include "normal" scene