use active of robot in scene as busy-indicator of robot in model

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;

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;

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> ;
 

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()

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;
-:}

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 {

ros3rag.placeB/src/main/jastadd/WorldModelB.relast

 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? ;

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();

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()