again new try for position calculation

- still not perfect (oszillation possible)
- add a few more locations in configs

ros3rag.placeA/src/main/resources/regions-a-placeworld.json

 {
   "regions": [
     { "name": "G", "positions": ["target_Blue"] },
-    { "name": "I", "positions": ["target_Green"] }
+    { "name": "G", "positions": ["target_Green"] }
   ]
 }

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;

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
+        // (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);
         }
-        if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
-          System.out.println(pre + zoneName);
-          return zoneName;  // actually return (>) + 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);
+        }
+        // (6)
+        if (getCurrentPosition().startsWith("(^)") && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) {
+          return logAndReturn(pre, "safe");
         }
-        if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) {
-          System.out.println(pre + zoneName);
-          return zoneName;  // actually return (^) + targetZone
       }
+      // (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;
       }
     }

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;

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"
       ]
     }

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"] }