More correct diff and operation handling.

- add "rewind" topic to restart app
- update ragconnect to allow mqtt handler with ports added to hostname

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

@@ -72,14 +72,4 @@ public class Util {
     }
     return repoRoot.resolve(moduleName);
   }
-
-  private static class HostAndPort {
-    String host;
-    int port;
-  }
-
-  @FunctionalInterface
-  public interface HandleLink {
-    void handle(boolean isEndEffector, String topic, String name) throws IOException;
-  }
 }

ros3rag.common/src/main/resources/tasks.gradle

@@ -44,9 +44,7 @@ task grammar2uml(type: JavaExec) {
     args([
             '--verbose',
             'src/gen/jastadd/types.relast'
-    ]
-            +
-            project.ext.relastFiles)
+    ] + project.ext.relastFiles)
 }
 
 // phase: RelAst
@@ -62,9 +60,8 @@ task relastToJastAdd(type: JavaExec) {
           "--serializer=jackson",
           "--resolverHelper",
           "--file",
-          "src/gen/jastadd/types.relast"]
-            +
-            project.ext.relastFiles)
+          "src/gen/jastadd/types.relast"
+    ] + project.ext.relastFiles)
 
     inputs.files project.ext.relastFiles + ['src/gen/jastadd/types.relast']
     outputs.files file("./src/gen/jastadd/model.ast"), file("./src/gen/jastadd/model.jadd")

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

@@ -4,11 +4,14 @@ import de.tudresden.inf.st.placeA.ast.MqttHandler;
 import de.tudresden.inf.st.placeA.ast.Robot;
 import de.tudresden.inf.st.placeA.ast.Scene;
 import de.tudresden.inf.st.placeA.ast.WorldModelA;
+import de.tudresden.inf.st.ros3rag.common.Configuration;
 import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
 import java.nio.charset.StandardCharsets;
+import java.util.LinkedHashMap;
+import java.util.Map;
 import java.util.concurrent.CountDownLatch;
 import java.util.concurrent.TimeUnit;
 
@@ -25,6 +28,7 @@ public class MainA {
 
   private final static String TOPIC_MODEL = "place-a/model";
   private final static String TOPIC_STATUS = "place-a/status";
+  private final static String TOPIC_REWIND = "place-a/rewind";
   private final static String TOPIC_EXIT = "place-a/exit";
 
   private final static String TOPIC_SCENE_INIT = "place-a/scene/init";
@@ -36,6 +40,13 @@ public class MainA {
 
   private MqttHandler mainHandler;
   private WorldModelA model;
+  private Configuration config;
+
+  private static final Map<String, String> channelDescriptions = new LinkedHashMap<>() {{
+    put(TOPIC_MODEL, "Print current model (detailed if message starts with 'detail");
+    put(TOPIC_REWIND, "Rewind app to start");
+    put(TOPIC_EXIT, "Exit app");
+  }};
 
   public static void main(String[] args) throws Exception {
     new MainA().run(args);
@@ -46,9 +57,7 @@ public class MainA {
 
     // --- No configuration below this line ---
 
-    final var config = Util.parseConfig(UtilA.pathToDirectoryOfPlaceA().resolve(configFile).toFile());
-
-    model = new WorldModelA();
+    config = Util.parseConfig(UtilA.pathToDirectoryOfPlaceA().resolve(configFile).toFile());
 
     /// Prepare main handler
     mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
@@ -57,6 +66,15 @@ public class MainA {
     mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
     mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
     mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
+    mainHandler.newConnection(TOPIC_REWIND, bytes ->
+        {
+          try {
+            rewind("rewind");
+          } catch (Exception e) {
+            logger.catching(e);
+          }
+        }
+    );
 
     mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_BLUE, bytes ->
         UtilA.updatePositionOfObjectToLocation(model.getScene(), "objectRed1", "binBlue")
@@ -65,6 +83,18 @@ public class MainA {
       UtilA.updatePositionOfObjectToLocation(model.getScene(), "objectRed1", "binRed")
     );
 
+    logger.info("Supported commands: {}", channelDescriptions);
+
+    rewind("Start");
+
+    Runtime.getRuntime().addShutdownHook(new Thread(this::close));
+
+    exitCondition.await();
+  }
+
+  private void rewind(String statusMessage) throws Exception {
+    model = new WorldModelA();
+
     /// Reading scene and robot
     de.tudresden.inf.st.ceti.Scene scene = readScene(
         UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene)
@@ -83,17 +113,9 @@ public class MainA {
     model.connectScene(mqttUri(TOPIC_SCENE_UPDATE_FROM_ROS, config));
     model.getScene().connectLogicalScene(mqttUri(TOPIC_SCENE_UPDATE_TO_PLACE_B, config), true);
 
-    logStatus("Start");
-
-    logger.info("To print the current model states, send a message to the topic '{}'.", TOPIC_MODEL);
-    logger.info("To exit the system cleanly, send a message to the topic '{}', or use Ctrl+C.", TOPIC_EXIT);
+    logStatus(statusMessage);
 
     mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray());
-//    logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT);
-
-    Runtime.getRuntime().addShutdownHook(new Thread(this::close));
-
-    exitCondition.await();
   }
 
   private void setRobot(String name) {

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

@@ -21,25 +21,28 @@ aspect Computation {
         result.add(diff);
         continue;
       }
-      if (myObject.hasLocatedAt()) {
-        if (!otherObject.hasLocatedAt()) {
+      if (myObject.hasLocatedAt() && !otherObject.hasLocatedAt()) {
         var diff = new DifferenceObjectMisplaced();
         diff.setObject(myObject);
         diff.setPreviousLocation(myObject.getLocatedAt());
         result.add(diff);
         continue;
       }
-        if (!myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName())) {
-          // object at different locations
+      if (!myObject.hasLocatedAt() && !otherObject.hasLocatedAt() ||
+          (myObject.hasLocatedAt() && otherObject.hasLocatedAt() &&
+           myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName()))) {
+        // no diff if otherObject has no location information, and if both objects are located at same location
+        // otherObject is always non-null, so this case does not need to be treated
+        continue;
+      }
       var difference = new DifferenceObjectAtWrongPlace();
       difference.setObject(myObject);
+      if (myObject.hasLocatedAt()) {
         difference.setPreviousLocation(myObject.getLocatedAt());
+      }
       difference.setNewLocation(otherObject.getLocatedAt());
       result.add(difference);
     }
-      }
-      // other difference not handled yet, e.g. object deleted, or objects having no locations
-    }
     return result;
   }
 
@@ -60,7 +63,9 @@ aspect Computation {
     Set<Robot> robotsFitForPreviousLocation = new HashSet<>();
     Set<Robot> robotsFitForNewLocation = new HashSet<>();
     for (var robot : worldModelB().getRobotList()) {
-      if (robot.canReach(getPreviousLocation().getName())) {
+      // check if robot can reach both, location (if any) and the object
+      if ((!hasPreviousLocation() || robot.canReach(getPreviousLocation().getName())) &&
+           robot.canReach(getObject().getName())) {
         robotsFitForPreviousLocation.add(robot);
       }
       if (robot.canReach(getNewLocation().getName())) {
@@ -68,7 +73,13 @@ aspect Computation {
       }
     }
     if (robotsFitForPreviousLocation.isEmpty()) {
-      return Collections.singletonList(new ErrorOperation("No robot can reach previous location " + getPreviousLocation().getName()));
+      StringBuilder sb = new StringBuilder("No robot can reach object ");
+      sb.append(getObject().getName());
+      if (hasPreviousLocation()) {
+        sb.append(" and ").append(getPreviousLocation().getName());
+      }
+      sb.append("!");
+      return Collections.singletonList(new ErrorOperation(sb.toString()));
     }
     if (robotsFitForNewLocation.isEmpty()) {
       return Collections.singletonList(new ErrorOperation("No robot can reach new location " + getNewLocation().getName()));
@@ -178,13 +189,13 @@ aspect Printing {
 
   syn String Difference.prettyPrint();
   eq DifferenceObjectAtWrongPlace.prettyPrint() {
-    return "+DifferenceObjectAtWrongPlace of " + getObject().prettyPrint() + ": " + getPreviousLocation().prettyPrint() + " -> " + getNewLocation().prettyPrint() + "+";
+    return "-DifferenceObjectAtWrongPlace of " + getObject().prettyPrint() + ": " + (hasPreviousLocation() ? getPreviousLocation().prettyPrint() : "_") + " -> " + getNewLocation().prettyPrint() + "-";
   }
   eq DifferenceNewObject.prettyPrint() {
-    return "+DifferenceNewObject of " + getObject().prettyPrint() + "+";
+    return "-DifferenceNewObject of " + getObject().prettyPrint() + "-";
   }
   eq DifferenceObjectMisplaced.prettyPrint() {
-    return "+DifferenceObjectMisplaced of " + getObject().prettyPrint() + ": " + getPreviousLocation().prettyPrint() + "+";
+    return "-DifferenceObjectMisplaced of " + getObject().prettyPrint() + ": " + getPreviousLocation().prettyPrint() + "-";
   }
 
   syn String Operation.prettyPrint();

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

@@ -5,7 +5,7 @@ Robot ::= <Name:String> CanReachObjectOfInterestWrapper ;
 abstract Difference ;
 rel Difference.Object -> LogicalMovableObject ;
 DifferenceObjectAtWrongPlace : Difference ;
-rel DifferenceObjectAtWrongPlace.PreviousLocation -> LogicalDropOffLocation ;
+rel DifferenceObjectAtWrongPlace.PreviousLocation? -> LogicalDropOffLocation ;
 rel DifferenceObjectAtWrongPlace.NewLocation -> LogicalDropOffLocation ;
 DifferenceNewObject : Difference ;
 DifferenceObjectMisplaced : Difference ;

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

@@ -7,9 +7,12 @@ import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
+import java.io.IOException;
 import java.nio.charset.StandardCharsets;
 import java.nio.file.Path;
 import java.nio.file.Paths;
+import java.util.LinkedHashMap;
+import java.util.Map;
 import java.util.concurrent.CountDownLatch;
 import java.util.concurrent.TimeUnit;
 
@@ -26,6 +29,7 @@ public class MainB {
 
   private final static String TOPIC_MODEL = "place-b/model";
   private final static String TOPIC_STATUS = "place-b/status";
+  private final static String TOPIC_REWIND = "place-b/rewind";
   private final static String TOPIC_EXIT = "place-b/exit";
 
   private final static String TOPIC_MY_SCENE_INIT = "place-b/scene/init";
@@ -39,6 +43,13 @@ public class MainB {
 
   private MqttHandler mainHandler;
   private WorldModelB model;
+  private Configuration config;
+
+  private static final Map<String, String> channelDescriptions = new LinkedHashMap<>() {{
+    put(TOPIC_MODEL, "Print current model (detailed if message starts with 'detail");
+    put(TOPIC_REWIND, "Rewind app to start");
+    put(TOPIC_EXIT, "Exit app");
+  }};
 
   public static void main(String[] args) throws Exception {
     new MainB().run(args);
@@ -49,8 +60,7 @@ public class MainB {
 
     // --- No configuration below this line ---
 
-    final var config = Util.parseConfig(UtilB.pathToDirectoryOfPlaceB().resolve(configFile).toFile());
-    model = new WorldModelB();
+    config = Util.parseConfig(UtilB.pathToDirectoryOfPlaceB().resolve(configFile).toFile());
 
     /// Prepare main handler
     mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
@@ -59,11 +69,32 @@ public class MainB {
     CountDownLatch exitCondition = new CountDownLatch(1);
     mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
     mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
+    mainHandler.newConnection(TOPIC_REWIND, bytes ->
+        {
+          try {
+            rewind("rewind");
+          } catch (Exception e) {
+            logger.catching(e);
+          }
+        }
+    );
 
     mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
         UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed")
     );
 
+    logger.info("Supported commands: {}", channelDescriptions);
+
+    rewind("Start");
+
+    Runtime.getRuntime().addShutdownHook(new Thread(this::close));
+
+    exitCondition.await();
+  }
+
+  private void rewind(String statusMessage) throws Exception {
+    model = new WorldModelB();
+
     /// Reading scene and robots
     de.tudresden.inf.st.ceti.Scene scene = readScene(
         UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene)
@@ -99,17 +130,9 @@ public class MainB {
     model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config));
     model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false);
 
-    logStatus("Start");
-
-    logger.info("To print the current model states, send a message to the topic '{}'.", TOPIC_MODEL);
-    logger.info("To exit the system cleanly, send a message to the topic '{}', or use Ctrl+C.", TOPIC_EXIT);
+    logStatus(statusMessage);
 
     mainHandler.publish(TOPIC_MY_SCENE_INIT, scene.toByteArray());
-//    logger.fatal("Skipping publishing to {} for now", TOPIC_MY_SCENE_INIT);
-
-    Runtime.getRuntime().addShutdownHook(new Thread(this::close));
-
-    exitCondition.await();
   }
 
   private void logStatus(String message) {