diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect
index dd61bb52227a984bdb2c512505b56b33727777ea..28bc80d7f9d7b429feccf9c933bd95fd3943c2ee 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 2d8077469bcf93f69ccef4777e6093b152c6e870..91f383e5b77ee77e32903579e8f1321bdf294f6c 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 c31c1c3dc8e251c6e6fc3989b2e860db6ec3ee2a..8061ea582af15f8eb156926916f36135f364b51c 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 93adc36cd2233a6b90140e2817c1cb8d17e6df86..97a95cb4cab6ade325a28991ebacbec5fea4b608 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 379efaa7e245561d55428934cb11c9199f3907dd..2ef0239e0bce9b2bb6a4290a109c4a1a6294f182 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 217dc1823213bba3de120ae7a0f82226166a5976..3d7f79e1b31b282b4d17bf4ed575a79b1363bfd0 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 856d1b0ca407957232e499421301db70f223f332..e6bb98b441d535ed8ad8be411a492c9f7dbf5458 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 9b8602b279e67de08ce0d1b43e02ec0dab4eeeee..281acccf95245e6cefc049543cebe8988669182d 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 8ad4ec64df1789f4bddc3c96299a3cc7bc2221c4..d58dc9e8c0599ac94d33e32ba93d7f851c5376ec 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()