diff --git a/ros3rag.placeA/src/main/resources/regions-a-placeworld.json b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json
index 43cb170518c8f0e817ad26b47b719317f75a163d..7b9e155e0f56bc006adb6bb4821ecec998ad74d2 100644
--- a/ros3rag.placeA/src/main/resources/regions-a-placeworld.json
+++ b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json
@@ -1,6 +1,6 @@
{
"regions": [
{ "name": "G", "positions": ["target_Blue"] },
- { "name": "I", "positions": ["target_Green"] }
+ { "name": "G", "positions": ["target_Green"] }
]
}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index 2c88a864d233dd9d023fcac06990ec10e73caf11..8d91fc98c41cc625abcea7162bad571c134466d6 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -100,11 +100,22 @@ CommandCheckForOccupiedCollaborationZone maps byte[] bytes to String {:
reject();
}
+ // if target is collaboration zone
ObjectOfInterest obj = scene.resolveObjectOfInterest(pp.getIdPlace());
DropOffLocation loc = obj.asDropOffLocation();
if (loc.isCollaborationZone()) {
- CollaborationZone cz = loc.asCollaborationZone();
- String specialIncomingName = "(>)" + cz.getName();
+ String specialIncomingName = "(>)" + loc.getName();
+ if (!occupiedZoneNames.contains(specialIncomingName)) {
+ occupiedZoneNames.add(specialIncomingName);
+ updated = true;
+ }
+ }
+ // if object is located at collaboration zone
+ LogicalObjectOfInterest logicalObj = scene.getLogicalScene().resolveLogicalObjectOfInterest(pp.getIdPick());
+ LogicalMovableObject toBePicked = logicalObj.asLogicalMovableObject();
+ DropOffLocation sourceLocation = toBePicked.myLocation();
+ if (sourceLocation != null && sourceLocation.isCollaborationZone()) {
+ String specialIncomingName = "(^)" + sourceLocation.getName();
if (!occupiedZoneNames.contains(specialIncomingName)) {
occupiedZoneNames.add(specialIncomingName);
updated = true;
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index e11e27bc4e2fe39072d0c30b81539ae9880150b9..420813b65a6093ab57b1d76da4bcca7542db7ebc 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -314,29 +314,43 @@ aspect Computation {
for (String zoneName : arrayAsList(getOccupiedCollaborationZoneNames())) {
if (zoneName.startsWith("(>)")) {
String targetZone = zoneName.substring(3);
- if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
- System.out.println(pre + targetZone);
- return targetZone; // robot was moving and has now reached its target
- }
- if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
- System.out.println(pre + zoneName);
- return zoneName; // actually return (>) + targetZone
+ // (1)
+ String currentTarget = getCurrentPosition().startsWith("(") ? getCurrentPosition().substring(3) : getCurrentPosition();
+ if (!targetZone.equals(currentTarget) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
+ return logAndReturn(pre, "(+)" + targetZone);
}
}
+ // (2) // getCurrentPosition().startsWith("(+)") &&
+ if (isStateMoving(myState)) {
+ return logAndReturn(pre, zoneName);
+ }
+ // (3)
+ if (getCurrentPosition().startsWith("(>)") && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
+ return logAndReturn(pre, getCurrentPosition().substring(3));
+ }
if (zoneName.startsWith("(^)")) {
- String targetZone = zoneName.substring(3);
- if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
- System.out.println(pre + "(empty)");
- return ""; // robot was moving and has now reached its target
+ String sourceZone = zoneName.substring(3);
+ // (4)
+ if (!getCurrentPosition().startsWith("(^)") && !getCurrentPosition().equals("safe") && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
+ return logAndReturn(pre, "(-)" + sourceZone);
}
- if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
- System.out.println(pre + zoneName);
- return zoneName; // actually return (^) + targetZone
+ // (6)
+ if (getCurrentPosition().startsWith("(^)") && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
+ return logAndReturn(pre, "safe");
}
}
+ // (5)
+ if (getCurrentPosition().startsWith("(-)") && isStateMoving(myState)) {
+ return logAndReturn(pre, "(^)" + getCurrentPosition().substring(3));
+ }
}
- System.out.println(pre + getCurrentPosition());
- return getCurrentPosition();
+ // no-op
+ return logAndReturn(pre, getCurrentPosition());
+ }
+
+ private String Robot.logAndReturn(String pre, String returnValue) {
+ System.out.println(pre + returnValue);
+ return returnValue;
}
private boolean Robot.isStateMoving(de.tudresden.inf.st.ceti.Object.State state) {
@@ -404,16 +418,9 @@ aspect Navigation {
syn Robot CollaborationZone.occupient() {
// (>) == moving in, (^) == evacuate/moving out
for (Robot robot : worldModelB().getRobotList()) {
- String zoneName = robot.getCurrentPosition();
- if (zoneName.startsWith("(>)") && zoneName.substring(3).equals(getName())) {
- // robot is currently moving into this zone. always regard it as occupied by it
- return robot;
- }
- if (zoneName.startsWith("(^)") && zoneName.substring(3).equals(getName())) {
- // robot is moving out of this zone. also regard it as occupied
- return robot;
- }
+ String zoneName = robot.getCurrentPosition().startsWith("(") ? robot.getCurrentPosition().substring(3) : robot.getCurrentPosition();
if (zoneName.equals(getName())) {
+ // robot is currently in this zone, or moving into or out of this zone. always regard it as occupied by it
return robot;
}
}
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 2c0da972e5388c7a1adc87a845561efb05562ce7..52f1a58a07bcf2728f8c0d2f8448e5506d182bf4 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
@@ -45,6 +45,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
protected void createSpecificMainHandlerConnections() {
mainHandler.newConnection("place-b/demo", bytes -> {
String topic = new String(bytes);
+ int slashIndex;
switch (topic) {
case "objectRed1/red":
UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed");
@@ -65,29 +66,32 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
case "arm2/picking":
case "arm2/placing":
case "arm2/moving":
- int slashIndex = topic.indexOf("/");
+ slashIndex = topic.indexOf("/");
String robot = topic.substring(0, slashIndex);
String stateString = "STATE_" + topic.substring(slashIndex + 1).toUpperCase();
Object.State state = Object.State.valueOf(stateString);
updateAndPublishScene(robot, r -> r.toBuilder().setState(state).build());
break;
- case "big-blue/cz":
- updateAndPublishScene("bigBlue", bigBlue -> {
- Object.Builder builder = bigBlue.toBuilder();
+ 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);
+ Position pos = model.getMyScene().resolveObjectOfInterest(location).asDropOffLocation().getPosition();
+ updateAndPublishScene(obj, o -> {
+ Object.Builder builder = o.toBuilder();
builder.getPosBuilder()
- .setX(0.0012)
- .setY(0.65)
- .setZ(0.94);
- return builder.build();
- });
- break;
- case "big-blue/g1":
- updateAndPublishScene("bigBlue", bigBlue -> {
- Object.Builder builder = bigBlue.toBuilder();
- builder.getPosBuilder()
- .setX(0.1)
- .setY(1.0)
- .setZ(0.93);
+ .setX(pos.getX())
+ .setY(pos.getY())
+ .setZ(pos.getZ());
return builder.build();
});
break;
diff --git a/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json b/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json
index 29f63f1969054177fca57102dc5f5988f54e169c..7f5d47fc8419d5d4535944493949f1d1672e651a 100644
--- a/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json
+++ b/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json
@@ -13,7 +13,7 @@
"name": "arm2",
"reachableLocations": [
"cz1",
- "G1",
+ "G1", "G2", "G3", "G4",
"I1", "I2", "I3" ,"I4"
]
}
diff --git a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
index 224d69e448eb070ba67c6beaa6795c783d352c84..19c849b41643e12b82ebc715c1949b627c37a68a 100644
--- a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
+++ b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json
@@ -6,7 +6,7 @@
{ "name": "D", "positions": ["D1"] },
{ "name": "E", "positions": ["E1"] },
{ "name": "F", "positions": ["F1"] },
- { "name": "G", "positions": ["G1"] },
+ { "name": "G", "positions": ["G1", "G2", "G3", "G4"] },
{ "name": "H", "positions": ["H1"] },
{ "name": "I", "positions": ["I1", "I2", "I3", "I4"] },
{ "name": "CZ", "positions": ["cz1"] }