working on real collaboration

- update config structure
- fixed exit and rewind
- reread config on rewind
- read regions on both sites
- add demo commands for site B
- create svg at model-command for site-B, and publish filename
- enhance compute-operations: source location can be collaboration zone as well, add wait command when zone is not owned and occupied
- connect occupiedCollaborationZoneNames with self-loop and "moving-in" and "moving-out" logic

ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java

@@ -12,13 +12,19 @@ import java.util.List;
 @JsonIgnoreProperties(ignoreUnknown = true)
 public class Configuration {
   public String mqttHost;
-  public String filenameInitialSceneA;
   public String filenameRegions;
   public String coordinatorMqttTopicPrefix;
-  public List<ReachabilityConfig> reachability;
+  public ConfigurationForA forA;
+  public ConfigurationForB forB;
 
-  public static class ReachabilityConfig {
-    public String filename;
+  public static class ConfigurationForA {
+    public String filenameInitialScene;
+  }
+
+  public static class ConfigurationForB {
+    public List<String> reachability;
+    public List<String> topicsSceneUpdate;
+    public String topicCommand;
   }
 
   public boolean useCoordinator() {

ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java

@@ -65,18 +65,21 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
     mainHandler.newConnection(TOPIC_EXIT, bytes -> {
       logger.info("Got exit command");
       exitCondition.countDown();
+      startCondition.countDown();
       logger.debug("exit latch count = {}, start latch count= {}",
           exitCondition.getCount(), startCondition.getCount());
     });
     mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
     mainHandler.newConnection(TOPIC_REWIND, bytes ->
-        {
-          try {
-            rewind("rewind");
-          } catch (Exception e) {
-            logger.catching(e);
+        new Thread(() -> {
+          {
+            try {
+              rewind("rewind");
+            } catch (Exception e) {
+              logger.catching(e);
+            }
           }
-        }
+        }).start()
     );
     if (config.useCoordinator()) {
       logger.info("Using coordinator logic");
@@ -136,11 +139,14 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
       logger.debug("Closing previous connections for {}", cellName);
       model.ragconnectCloseConnections();
     }
+
+    config = Util.parseConfig(pathToConfig.toFile());
+
     logger.debug("Creating world model for {}", cellName);
     model = createWorldModel();
 
     logger.debug("Reading robots for {}", cellName);
-    readRobotsAndReachability();
+    readInitialConfigs();
 
     logger.debug("Setup model connection for {}", cellName);
     model.ragconnectCheckIncremental();
@@ -172,7 +178,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa
 
   protected abstract WorldModel createWorldModel() throws Exception;
 
-  protected abstract void readRobotsAndReachability() throws Exception;
+  protected abstract void readInitialConfigs() throws Exception;
 
   protected abstract void connectEndpoints() throws IOException;
 

ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java

@@ -4,7 +4,9 @@ import de.tudresden.inf.st.placeA.ast.MqttHandler;
 import de.tudresden.inf.st.placeA.ast.Scene;
 import de.tudresden.inf.st.placeA.ast.WorldModelA;
 import de.tudresden.inf.st.ros3rag.common.SharedMainParts;
+import de.tudresden.inf.st.ros3rag.common.Util;
 
+import java.io.File;
 import java.io.IOException;
 
 import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
@@ -62,14 +64,18 @@ public class MainA extends SharedMainParts<MqttHandler, WorldModelA> {
   @Override
   protected WorldModelA createWorldModel() throws Exception {
     de.tudresden.inf.st.ceti.Scene scene = readScene(
-        UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialSceneA)
+        UtilA.pathToDirectoryOfPlaceA().resolve(config.forA.filenameInitialScene)
     );
     Scene myScene = UtilA.convert(scene);
     return new WorldModelA().setScene(myScene);
   }
 
   @Override
-  protected void readRobotsAndReachability() {
+  protected void readInitialConfigs() throws IOException {
+    // read and set regions
+    File regionFile = UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameRegions).toFile();
+    UtilA.setRegions(model, Util.parseRegionConfig(regionFile));
+
     // no robots to be set
   }
 

ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java

 package de.tudresden.inf.st.placeA;
 
 import de.tudresden.inf.st.placeA.ast.*;
+import de.tudresden.inf.st.ros3rag.common.RegionConfiguration;
+import de.tudresden.inf.st.ros3rag.common.RegionConfiguration.RegionDefinition;
 import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
@@ -23,6 +25,17 @@ public class UtilA {
     return new ExposingASTNode().exposed_apply_ConvertScene(scene);
   }
 
+  static void setRegions(WorldModelA model, RegionConfiguration config) {
+    JastAddList<Region> result = new JastAddList<>();
+    for (RegionDefinition def : config.regions) {
+      Region region = new Region();
+      region.setName(def.name);
+      region.setLocationNames(String.join(",", def.positions));
+      result.add(region);
+    }
+    model.setRegionList(result);
+  }
+
   static void updatePositionOfObjectToLocation(Scene scene, String objName, String locationName) {
     ObjectOfInterest obj = scene.resolveObjectOfInterest(objName);
     ObjectOfInterest location = scene.resolveObjectOfInterest(locationName);

ros3rag.placeA/src/main/resources/config-a-placeworld.yaml

+mqttHost: "192.168.0.122"
+filenameRegions: "src/main/resources/regions-a-placeworld.json"
+forA:
+  filenameInitialScene: "src/main/resources/config-scene-a-placeworld.json"
+coordinatorMqttTopicPrefix: "coordinating/rag-a"

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

@@ -4,6 +4,7 @@ receive indexed WorldModelB.OtherScene ;
 receive WorldModelB.TestingOtherScene ;
 receive Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
 receive Robot.OwnedCollaborationZoneNames using ConfigChangeCommandCheckForOwnedCollaborationZone ;
+receive Robot.OccupiedCollaborationZoneNames using CommandCheckForOccupiedCollaborationZone ;
 
 // --- sending ---
 send WorldModelB.NextOperation using PrintOperation ;
@@ -27,7 +28,7 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
   if (!command.hasConfigChange()) {
     reject();
   }
-  System.out.println(command);
+//  System.out.println(command);
   de.tudresden.inf.st.ceti.ConfigChange cc = command.getConfigChange();
   Scene scene = worldModelB().getMyScene();
   ObjectOfInterest obj = scene.resolveObjectOfInterest(cc.getIdCollaborationZone());
@@ -49,3 +50,71 @@ ConfigChangeCommandCheckForOwnedCollaborationZone maps byte[] bytes to String {:
   }
   return String.join(",", collaborationZoneNames);
 :}
+
+CommandCheckForOccupiedCollaborationZone maps byte[] bytes to String {:
+  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();
+  }
+  // if the command is not the correct command type, do not change anything
+  if (!command.hasPickAndPlace() && !command.hasEvacuate()) {
+    reject();
+  }
+//  System.out.println(command);
+
+  Scene scene = worldModelB().getMyScene();
+  Robot thisRobot = (Robot) this;
+
+//  // get current value, but remove every "moving out" zone
+//  List<String> occupiedZoneNames = thisRobot.occupiedCollaborationZonesAsList().stream()
+//    .filter(!zone -> zone.startsWith("(^)"))
+//    .collect(java.util.stream.Collectors.toList());
+
+  // with a new command, begin with a clean occ list
+  List<String> occupiedZoneNames = new java.util.ArrayList<>();
+
+  // (>) == moving in, (^) == evacuate/moving out
+  boolean updated = false;
+  if (command.hasPickAndPlace()) {
+    de.tudresden.inf.st.ceti.PickAndPlace pp = command.getPickAndPlace();
+
+    // if the command is not executed by this robot, do not change anything
+    if (!thisRobot.getName().equals(pp.getIdRobot())) {
+      reject();
+    }
+
+    ObjectOfInterest obj = scene.resolveObjectOfInterest(pp.getIdPlace());
+    DropOffLocation loc = obj.asDropOffLocation();
+    if (loc.isCollaborationZone()) {
+      CollaborationZone cz = loc.asCollaborationZone();
+      String specialIncomingName = "(>)" + cz.getName();
+      if (!occupiedZoneNames.contains(specialIncomingName)) {
+        occupiedZoneNames.add(specialIncomingName);
+        updated = true;
+      }
+    }
+  } else if (command.hasEvacuate()) {
+    de.tudresden.inf.st.ceti.Evacuate ev = command.getEvacuate();
+
+    // if the command is not executed by this robot, do not change anything
+    if (!thisRobot.getName().equals(ev.getIdRobot())) {
+      reject();
+    }
+    ObjectOfInterest obj = scene.resolveObjectOfInterest(ev.getIdCollaborationZone());
+    DropOffLocation loc = obj.asDropOffLocation();
+    if (loc.isCollaborationZone()) {
+      CollaborationZone cz = loc.asCollaborationZone();
+      String specialEvacuateName = "(^)" + cz.getName();
+      if (!occupiedZoneNames.contains(specialEvacuateName)) {
+        occupiedZoneNames.add(specialEvacuateName);
+        updated = true;
+      }
+    }
+  }
+  if (!updated) {
+    reject();
+  }
+  return String.join(",", occupiedZoneNames);
+:}

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

@@ -165,22 +165,43 @@ aspect Computation {
               Vertex transit = previousVertex;
               for (Edge edge : shortestPath) {
                 Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
+                DropOffLocation sourceLocation = worldModelB().getMyScene().resolveObjectOfInterest(previousVertex.asVertexWithObjectOfInterest().getNameOfObjectOfInterest()).asDropOffLocation();
                 DropOffLocation targetLocation = worldModelB().getMyScene().resolveObjectOfInterest(target.asVertexWithObjectOfInterest().getNameOfObjectOfInterest()).asDropOffLocation();
 //                Robot executingRobot = worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot());
                 worldModelB().findRobot(edge.asEdgeWithRobot().getNameOfRobot()).ifPresentOrElse(executingRobot -> {
+                  /* for both locations, in case it is a collaboration zone, we need to ensure the following:
+                     (a) claimed by the executing robot, and
+                     (b) not occupied by another robot, and
+                     (c) free (as no movable object is contained) -- this should be reflected in the graph!
+                  */
+                  if (sourceLocation.isCollaborationZone()) {
+                    CollaborationZone cz = sourceLocation.asCollaborationZone();
+                    System.out.println("source is cz, occ:" + (cz.hasOccupient() ? cz.occupient().nameAndHash() : "none") + ", owner: " + (cz.hasOwner() ? cz.owner().nameAndHash() : "none"));
+                    // order is important here, first add Evacuate, then ConfigChange
+                    if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+                      result.add(Evacuate.of(cz.occupient(), cz));
+                    }
+                    if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
+                      if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+                        result.add(Wait.of());
+                      } else {
+                        result.add(ConfigChange.of(executingRobot, cz));
+                      }
+                    }
+                  }
                   if (targetLocation.isCollaborationZone()) {
-                    /* here we need to check, if the collaboration zone is
-                       (a) claimed by the executing robot, and
-                       (b) occupied by another robot, and
-                       (c) free (as no movable object is contained) -- this should be reflected in the graph!
-                    */
                     CollaborationZone cz = targetLocation.asCollaborationZone();
+                    System.out.println("target is cz, occ:" + (cz.hasOccupient() ? cz.occupient().nameAndHash() : "none") + ", owner: " + (cz.hasOwner() ? cz.owner().nameAndHash() : "none"));
                     // order is important here, first add Evacuate, then ConfigChange
                     if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
                       result.add(Evacuate.of(cz.occupient(), cz));
                     }
                     if (!cz.hasOwner() || (cz.hasOwner() && !cz.owner().equals(executingRobot))) {
-                      result.add(ConfigChange.of(executingRobot, cz));
+                      if (cz.hasOccupient() && !cz.occupient().equals(executingRobot)) {
+                        result.add(Wait.of());
+                      } else {
+                        result.add(ConfigChange.of(executingRobot, cz));
+                      }
                     }
                   }
                   result.add(PickAndPlace.of(executingRobot, getObject(), targetLocation));
@@ -229,6 +250,10 @@ aspect Computation {
     return result;
   }
 
+  public static Wait Wait.of() {
+    return new Wait();
+  }
+
   //--- getNextOperation ---
   syn Operation WorldModelB.getNextOperation() {
     if (diffToOperations().getNumChild() == 0) {
@@ -306,6 +331,7 @@ aspect AttributeMappings {
           .setIdCollaborationZone(getCollaborationZone().getName())
           .build())
         .build().toByteArray();
+  eq Wait.toProtobufByteArray() = null;
 }
 
 aspect Navigation {
@@ -323,9 +349,20 @@ aspect Navigation {
 //  eq WorldModelB.getChild().allMappings() = getLocationMapping();
   syn boolean CollaborationZone.hasOccupient() = occupient() != null;
   syn Robot CollaborationZone.occupient() {
+    // (>) == moving in, (^) == evacuate/moving out
     for (Robot robot : worldModelB().getRobotList()) {
-      if (getName().equals(robot.getOccupiedCollaborationZoneName())) {
-        return robot;
+      for (String zoneName : arrayAsList(robot.getOccupiedCollaborationZoneNames())) {
+        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.isBusy()) {
+          // robot is busy moving out of this zone. regard it as occupied as long as the robot is busy
+          return robot;
+        }
+        if (zoneName.equals(getName())) {
+          return robot;
+        }
       }
     }
     return null;
@@ -333,7 +370,7 @@ aspect Navigation {
   syn boolean CollaborationZone.hasOwner() = owner() != null;
   syn Robot CollaborationZone.owner() {
     for (Robot robot : worldModelB().getRobotList()) {
-      List<String> collaborationZoneNames = Arrays.asList(robot.getOwnedCollaborationZoneNames().split(","));
+      List<String> collaborationZoneNames = arrayAsList(robot.getOwnedCollaborationZoneNames());
       if (collaborationZoneNames.contains(getName())) {
         return robot;
       }
@@ -341,6 +378,7 @@ aspect Navigation {
     return null;
   }
   syn List<String> Robot.ownedCollaborationZonesAsList() = arrayAsList(getOwnedCollaborationZoneNames());
+  syn List<String> Robot.occupiedCollaborationZonesAsList() = arrayAsList(getOccupiedCollaborationZoneNames());
   eq Region.locationList() {
     List<DropOffLocation> result = new ArrayList<>();
     if (!worldModelB().hasMyScene()) { return result; }
@@ -398,18 +436,24 @@ aspect Printing {
   eq Evacuate.prettyPrint() {
     return "+Evacuate: " + getRobotToExecute().prettyPrint() + " from " + getCollaborationZone().prettyPrint() + "+";
   }
+  eq Wait.prettyPrint() {
+    return "+Wait+";
+  }
 }
 
 aspect DumpAst {
-  public void WorldModelB.dumpAst(java.util.function.Consumer<de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder> options) {
+  public String WorldModelB.dumpAst(java.util.function.Consumer<de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder> options) {
     String now = java.time.LocalDateTime.now().format(java.time.format.DateTimeFormatter.ISO_LOCAL_DATE_TIME).replaceAll("[:\\.]", "-");
     try {
       de.tudresden.inf.st.jastadd.dumpAst.ast.DumpBuilder builder = de.tudresden.inf.st.jastadd.dumpAst.ast.Dumper.read(this);
       options.accept(builder);
       //builder.dumpAsPNG(java.nio.file.Paths.get("images/" + now + ".png"));
-      builder.dumpAsSVG(java.nio.file.Paths.get("images/" + now + ".svg"));
+      java.nio.file.Path path = java.nio.file.Paths.get("images/" + now + ".svg");
+      builder.dumpAsSVG(path);
+      return path.getFileName().toString();
     } catch (java.io.IOException e) {
       e.printStackTrace();
+      return null;
     }
   }
 }

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 <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneName> ;
+Robot ::= <Name:String> CanReachObjectOfInterestWrapper <OwnedCollaborationZoneNames> <OccupiedCollaborationZoneNames> ;
 // relations into nodes received by RagConnect are not allowed
 //rel Robot.OwnedCollaborationZone* <-> CollaborationZone.Owner? ;
 //rel Robot.OccupiedCollaborationZone? <-> CollaborationZone.Occupient? ;
@@ -34,3 +34,5 @@ rel ConfigChange.CollaborationZone -> CollaborationZone ;
 
 Evacuate : Operation ;
 rel Evacuate.CollaborationZone -> CollaborationZone ;
+
+Wait : Operation ;

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java

 package de.tudresden.inf.st.placeB;
 
+import de.tudresden.inf.st.ceti.Object;
 import de.tudresden.inf.st.ceti.Reachability;
 import de.tudresden.inf.st.placeB.ast.*;
 import de.tudresden.inf.st.ros3rag.common.Configuration;
 import de.tudresden.inf.st.ros3rag.common.SharedMainParts;
+import de.tudresden.inf.st.ros3rag.common.Util;
 
+import java.io.File;
 import java.io.IOException;
+import java.nio.charset.StandardCharsets;
 import java.nio.file.Path;
 import java.nio.file.Paths;
+import java.util.function.Function;
 
 import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
 
@@ -17,19 +22,16 @@ import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
  * @author rschoene - Initial contribution
  */
 public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
-  private final String TOPIC_MY_SCENE_UPDATE_FROM_ROS;
-  private final String TOPIC_COMMAND;
-  private final String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A;
   private final String TOPIC_DEMO_MOVE_objectRed1_RED;
+  private final String TOPIC_MODEL_SVG_PATH;
+
+  private de.tudresden.inf.st.ceti.Scene demo_scene;
 
   MainB(String configFile) {
     super("place-b", UtilB.pathToDirectoryOfPlaceB().resolve(configFile));
-    this.TOPIC_MY_SCENE_UPDATE_FROM_ROS = cellName + "/scene/update";
-    this.TOPIC_COMMAND = cellName + "/command";
-
-    this.TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A = "place-a/logical/update";
 
     this.TOPIC_DEMO_MOVE_objectRed1_RED = cellName + "/demo/move/objectRed1/red";
+    this.TOPIC_MODEL_SVG_PATH = cellName + "/model/svg/path";
   }
 
   public static void main(String[] args) throws Exception {
@@ -47,6 +49,47 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
     mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
         UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed")
     );
+    mainHandler.newConnection("demo/initial_scene", bytes -> {
+      try {
+        demo_scene = Util.readScene(UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b-placeworld-manual.json"));
+        mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray());
+      } catch (IOException e) {
+        e.printStackTrace();
+      }
+    });
+    mainHandler.newConnection("demo/arm1/active", bytes ->
+        updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(true).build()));
+    mainHandler.newConnection("demo/arm1/inactive", bytes ->
+        updateAndPublishScene("arm1", robot -> robot.toBuilder().setActive(false).build()));
+    mainHandler.newConnection("demo/arm2/active", bytes ->
+        updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(true).build()));
+    mainHandler.newConnection("demo/arm2/inactive", bytes ->
+        updateAndPublishScene("arm2", robot -> robot.toBuilder().setActive(false).build()));
+    mainHandler.newConnection("demo/big-blue/cz", bytes ->
+        updateAndPublishScene("bigBlue", bigBlue -> {
+          Object.Builder builder = bigBlue.toBuilder();
+          builder.getPosBuilder()
+              .setX(0.0012)
+              .setY(0.65)
+              .setZ(0.94);
+          return builder.build();
+        })
+    );
+    mainHandler.newConnection("demo/big-blue/g1", bytes ->
+        updateAndPublishScene("bigBlue", bigBlue -> {
+          Object.Builder builder = bigBlue.toBuilder();
+          builder.getPosBuilder()
+              .setX(0.1)
+              .setY(1.0)
+              .setZ(0.93);
+          return builder.build();
+        })
+    );
+  }
+
+  private void updateAndPublishScene(String objectName, Function<Object, Object> change) {
+    UtilB.updateObject(demo_scene, objectName, change);
+    mainHandler.publish(config.forB.topicsSceneUpdate.get(0), demo_scene.toByteArray());
   }
 
   @Override
@@ -57,13 +100,17 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
   }
 
   @Override
-  protected void readRobotsAndReachability() throws Exception {
+  protected void readInitialConfigs() throws Exception {
     model.setMyScene(new Scene());
 
+    // read and set regions
+    File regionBFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameRegions).toFile();
+    UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
+
     // init robots and reachability
-    for (Configuration.ReachabilityConfig reachabilityConfig : config.reachability) {
+    for (String reachabilityFileName : config.forB.reachability) {
       // assumption: robots do not change during runtime, so we have stable connections
-      Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename));
+      Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
       Reachability reachability = UtilB.readReachability(path);
 
       Robot robot = UtilB.createRobot(reachability.getIdRobot());
@@ -76,17 +123,33 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
 
   @Override
   protected void connectEndpoints() throws IOException {
-    model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config));
-    model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config), 0);
-    model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false);
+    for (String topic : config.forB.topicsSceneUpdate) {
+      model.connectMyScene(mqttUri(topic, config));
+    }
+    model.connectOtherScene(mqttUri("place-a/logical/update", config), 0);
+    model.connectNextOperation(mqttUri(config.forB.topicCommand, config), false);
     for (Robot robot : model.getRobotList()) {
       // self-loop
-      robot.connectOwnedCollaborationZoneNames(mqttUri(TOPIC_COMMAND, config));
+      robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
+      robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config));
     }
   }
 
   @Override
   protected String getModelInfos(WorldModelB worldModelB, boolean detailed) {
+    try {
+      String filename = model.dumpAst(builder -> {
+        builder.excludeChildren("Orientation", "Size");
+        builder.excludeRelations("ContainedInRegion");
+        builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations");
+        builder.includeAttributes("realRegion", "computeOperations");
+        builder.includeNullNodes();
+      });
+      mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8));
+    } catch (Exception e) {
+      logger.catching(e);
+    }
+
     return UtilB.getModelInfos(model, detailed);
   }
 

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java

@@ -4,7 +4,6 @@ import de.tudresden.inf.st.ceti.Object;
 import de.tudresden.inf.st.ceti.Reachability;
 import de.tudresden.inf.st.placeB.ast.*;
 import de.tudresden.inf.st.ros3rag.common.Configuration;
-import de.tudresden.inf.st.ros3rag.common.Configuration.ReachabilityConfig;
 import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
@@ -18,7 +17,6 @@ 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;
 
 /**
@@ -64,7 +62,7 @@ public class SimpleMainB {
   @SuppressWarnings("unused" )
   Scenario[] allScenarios = new Scenario[] { s2022, sMini, sPlaceworld, sPlaceworldManual };
 
-  final Scenario scenario = sPlaceworldManual;
+  final Scenario scenario = sPlaceworld;
 
   public static void main(String[] args) throws Exception {
     System.out.println("Running SimpleMainB");
@@ -81,11 +79,10 @@ public class SimpleMainB {
     config.mqttHost = scenario.mqttHost;
     String filenameInitialSceneB = "src/main/resources/config-scene-b-" + scenario.suffix + ".json";
     config.filenameRegions = "src/main/resources/regions-b-" + scenario.suffix + ".json";
-    ReachabilityConfig reachabilityR1 = new ReachabilityConfig();
-    reachabilityR1.filename = "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json";
-    ReachabilityConfig reachabilityR2 = new ReachabilityConfig();
-    reachabilityR2.filename = "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json";
-    config.reachability = Arrays.asList(reachabilityR1, reachabilityR2);
+    config.forB.reachability = Arrays.asList(
+        "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json",
+        "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json"
+    );
 
     Configuration configA = new Configuration();
     String filenameInitialSceneA = "src/main/resources/config-scene-a-" + scenario.suffix + ".json";
@@ -108,20 +105,20 @@ public class SimpleMainB {
     mqttHandler.waitUntilReady(2, TimeUnit.SECONDS);
     model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
 
-    Scene myScene;
+    Scene myScene = new Scene();
     // read initial scene
     Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve(filenameInitialSceneB);
-    de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
-    if (scenario.loadAndChangeScenes) {
-      myScene = UtilB.convert(initialScene);
-    } else {
-      myScene = new Scene();
-
-      mqttHandler.newConnection("coordinating/rag-b/command", bytes -> {
-        if (new String(bytes).equals("start")) {
-          mqttHandler.publish(scenario.topicSceneUpdateB, initialScene.toByteArray());
-        }
-      });
+    if (initialSceneFile.toFile().exists()) {
+      de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
+      if (scenario.loadAndChangeScenes) {
+        myScene = UtilB.convert(initialScene);
+      } else {
+        mqttHandler.newConnection("coordinating/rag-b/command", bytes -> {
+          if (new String(bytes).equals("start")) {
+            mqttHandler.publish(scenario.topicSceneUpdateB, initialScene.toByteArray());
+          }
+        });
+      }
     }
     model.setMyScene(myScene);
 
@@ -130,9 +127,9 @@ public class SimpleMainB {
     UtilB.setRegions(model, Util.parseRegionConfig(regionBFile));
 
     // init robots and reachability
-    for (ReachabilityConfig reachabilityConfig : config.reachability) {
+    for (String reachabilityFileName : config.forB.reachability) {
       // assumption: robots do not change during runtime, so we have stable connections
-      Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename));
+      Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName));
       Reachability reachability = UtilB.readReachability(path);
 
       Robot robot = UtilB.createRobot(reachability.getIdRobot());
@@ -188,7 +185,9 @@ public class SimpleMainB {
     LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene();
     byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA);
 
-    if (scenario.loadAndChangeScenes) {
+    if (scenario.loadAndChangeScenes && initialSceneFile.toFile().exists()) {
+      de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile);
+
       describedWait(1, "send new logical scene" );
       mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend);
 
@@ -246,7 +245,7 @@ public class SimpleMainB {
       de.tudresden.inf.st.ceti.Scene scene,
       @SuppressWarnings("SameParameterValue" ) String objectName,
       Position newPosition) {
-    return updateObject(scene, objectName, obj -> {
+    return UtilB.updateObject(scene, objectName, obj -> {
       Object.Builder builder = obj.toBuilder();
       builder.getPosBuilder()
           .setX(newPosition.getX())
@@ -259,30 +258,7 @@ public class SimpleMainB {
   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)) {
-        newObj = change.apply(obj);
-        break;
-      }
-    }
-    if (newObj == null) {
-      logger.error("Did not find object {}!", objectName);
-    } else {
-      scene = scene.toBuilder().setObjects(index, newObj).build();
-      logger.info("Update {} in scene to:\n {}", objectName, newObj);
-    }
-    return scene;
+    return UtilB.updateObject(scene, objectName, obj -> obj.toBuilder().setActive(false).build());
   }
 
 

ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java

 package de.tudresden.inf.st.placeB;
 
 import com.google.protobuf.util.JsonFormat;
+import de.tudresden.inf.st.ceti.Object;
 import de.tudresden.inf.st.ceti.Reachability;
 import de.tudresden.inf.st.placeB.ast.*;
 import de.tudresden.inf.st.ros3rag.common.RegionConfiguration;
@@ -12,7 +13,8 @@ import org.apache.logging.log4j.Logger;
 import java.io.IOException;
 import java.nio.file.Files;
 import java.nio.file.Path;
-import java.util.stream.Collectors;
+import java.util.List;
+import java.util.function.Function;
 
 /**
  * Static utility methods used only for place B.
@@ -153,6 +155,29 @@ public class UtilB {
     model.setRegionList(result);
   }
 
+  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)) {
+        newObj = change.apply(obj);
+        break;
+      }
+    }
+    if (newObj == null) {
+      logger.error("Did not find object {}!", objectName);
+    } else {
+      scene = scene.toBuilder().setObjects(index, newObj).build();
+      logger.info("Update {} in scene to:\n {}", objectName, newObj);
+    }
+    return scene;
+  }
+
   @SuppressWarnings("rawtypes")
   static class ExposingASTNode extends ASTNode {
     public Scene exposed_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception {

ros3rag.placeB/src/main/resources/config-b-placeworld.yaml

-mqttHost: "localhost"
-useReachability: true
-reachability:
-  - filename: "src/main/resources/reachability-b-arm1-placeworld.json"
-  - filename: "src/main/resources/reachability-b-arm2-placeworld.json"
+mqttHost: "192.168.0.122"
+filenameRegions: "src/main/resources/regions-b-placeworld.json"
+forB:
+  topicsSceneUpdate:
+    - "/ceti_cell_placeworld/scene/update"
+    - "/ceti_cell_2_placeworld/scene/update"
+  topicCommand: "/ceti_cell_placeworld/command"
+  reachability:
+    - "src/main/resources/reachability-b-arm1-placeworld.json"
+    - "src/main/resources/reachability-b-arm2-placeworld.json"
 coordinatorMqttTopicPrefix: "coordinating/rag-b"

ros3rag.placeB/src/main/resources/regions-b-placeworld.json

@@ -4,6 +4,7 @@
     { "name": "B", "positions": ["B1", "B2"] },
     { "name": "C", "positions": ["C1"] },
     { "name": "G", "positions": ["G1"] },
-    { "name": "E", "positions": ["E1"] }
+    { "name": "E", "positions": ["E1"] },
+    { "name": "CZ", "positions": ["cz1"] }
   ]
 }