create main entry points, restructure utils

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

+package de.tudresden.inf.st.ros3rag.common;
+
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+
+import java.util.List;
+
+/**
+ * New Data class for initial configuration.
+ *
+ * @author rschoene - Initial contribution
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+public class Configuration {
+  public String mqttHost;
+  public String filenameInitialScene;
+  public boolean useReachability;
+  public List<ReachabilityConfig> reachability;
+
+  public static class ReachabilityConfig {
+    public String idRobot;
+    public String filename;
+  }
+}

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

-package de.tudresden.inf.st.ros3rag.common;
-
-import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
-
-import java.util.List;
-import java.util.Map;
-import java.util.SortedMap;
-
-/**
- * Data class for initial configuration.
- *
- * @author rschoene - Initial contribution
- */
-@JsonIgnoreProperties(ignoreUnknown = true)
-public class DataConfiguration {
-  public ActualConfiguration panda_mqtt_connector;
-  @JsonIgnoreProperties(ignoreUnknown = true)
-  public static class ActualConfiguration {
-    public String server = "tcp://localhost:1883";
-    public DataTopics topics;
-    public int zone_size;
-    public double robot_speed_factor;
-    public String robot_planning_mode = "fluid_path";
-    public String default_trajectory = "<none>";
-    public List<String> zones;
-    public Map<String, SortedMap<String, String>> parts;
-    public Map<String, SortedMap<String, String>> end_effectors;
-    public List<DataWorkPose> goal_poses;
-  }
-  public static class DataTopics {
-    public String robotConfig;
-    public String trajectory;
-    public String nextStep;
-  }
-  public static class DataWorkPose {
-    public String position;
-    public String orientation;
-    public String work;
-  }
-}

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

 package de.tudresden.inf.st.ros3rag.common;
 
 import com.fasterxml.jackson.core.JsonParser;
-import com.fasterxml.jackson.databind.DeserializationFeature;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
-import com.fasterxml.jackson.dataformat.yaml.YAMLParser;
 import com.google.protobuf.util.JsonFormat;
-import de.tudresden.inf.st.ros3rag.common.DataConfiguration.ActualConfiguration;
+import de.tudresden.inf.st.ceti.Object;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
 
 import java.io.File;
 import java.io.IOException;
-import java.net.URI;
 import java.nio.file.Files;
-import java.util.Map;
-import java.util.SortedMap;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
 
 /**
  * Helper method dealing with config.
@@ -21,59 +22,55 @@ import java.util.SortedMap;
  * @author rschoene - Initial contribution
  */
 public class Util {
+  private static final Logger logger = LogManager.getLogger(Util.class);
 
-  public static ActualConfiguration parseConfig(File configFile) throws IOException {
+  public static Configuration parseConfig(File configFile) throws IOException {
     System.out.println("Using config file: " + configFile.getAbsolutePath());
     ObjectMapper mapper = new ObjectMapper(
         new YAMLFactory().configure(JsonParser.Feature.ALLOW_YAML_COMMENTS, true)
     );
-    ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector;
-    URI serverUri = URI.create(config.server);
-    config.server = serverUri.getHost() + ":" + (serverUri.getPort() == -1 ? 1883 : serverUri.getPort())
-        + serverUri.getPath();
-    return config;
+    return mapper.readValue(configFile, Configuration.class);
   }
 
-  public static void setMqttHost(SetHost handler, ActualConfiguration config) throws IOException {
-    HostAndPort hostAndPort = split(config.server);
-    handler.setHost(hostAndPort.host, hostAndPort.port);
+  public static String mqttUri(String topic, Configuration config) {
+    return "mqtt://" + config.mqttHost + "/" + topic;
   }
 
-  public static String mqttUri(String topic, ActualConfiguration config) {
-    return "mqtt://" + config.server + "/" + topic;
+  public static de.tudresden.inf.st.ceti.Scene readScene(java.nio.file.Path path) throws java.io.IOException {
+    logger.debug("Reading scene from {}", path.toAbsolutePath());
+    var jsonString = Files.readString(path);
+    var builder = de.tudresden.inf.st.ceti.Scene.newBuilder();
+    JsonFormat.parser().merge(jsonString, builder);
+    return builder.build();
   }
 
-  public static void iterateLinks(HandleLink callback, ActualConfiguration config) throws IOException {
-    for (Map.Entry<String, SortedMap<String, String>> dataRobot : config.parts.entrySet()) {
-      String topicPrefix = dataRobot.getKey() + "/";
-      for (Map.Entry<String, String> dataLink : dataRobot.getValue().entrySet()) {
-        String name = dataLink.getKey();
-        callback.handle(false, topicPrefix + name, name);
+  public static List<String> extractRobotNames(de.tudresden.inf.st.ceti.Scene scene) {
+    List<String> result = new ArrayList<>();
+    for (var obj : scene.getObjectsList()) {
+      if (obj.getType().equals(Object.Type.ARM)) {
+        result.add(obj.getId());
       }
     }
-    for (Map.Entry<String, SortedMap<String, String>> dataRobot : config.end_effectors.entrySet()) {
-      String topicPrefix = dataRobot.getKey() + "/";
-      for (Map.Entry<String, String> dataLink : dataRobot.getValue().entrySet()) {
-        String name = dataLink.getKey();
-        callback.handle(true, topicPrefix + name, name);
-      }
-    }
-  }
-
-  private static HostAndPort split(String serverString) {
-    HostAndPort result = new HostAndPort();
-    String[] serverTokens = serverString.replace("tcp://", "").split(":");
-    result.host = serverTokens[0];
-    result.port = Integer.parseInt(serverTokens[1]);
     return result;
   }
 
+  public static Path pathToCommonDirectory() {
+    return pathToModuleDirectory("ros3rag.common");
+  }
 
-  public static de.tudresden.inf.st.ceti.Scene readScene(java.nio.file.Path path) throws java.io.IOException {
-    var jsonString = Files.readString(path);
-    var builder = de.tudresden.inf.st.ceti.Scene.newBuilder();
-    JsonFormat.parser().merge(jsonString, builder);
-    return builder.build();
+  public static Path pathToModuleDirectory(String moduleName) {
+    Path current = Paths.get("").toAbsolutePath();
+    String currentFileName = current.getFileName().toString();
+    Path repoRoot;
+    if (currentFileName.equals("ros3rag.placeA") || currentFileName.equals("ros3rag.placeB") || currentFileName.equals("ros3rag.common")) {
+      // we are in some module, use parent
+      repoRoot = current.getParent();
+    } else if (current.resolve(moduleName).toFile().exists()) {
+      repoRoot = current;
+    } else {
+      throw new IllegalStateException("Could not find ros3rag.common directory");
+    }
+    return repoRoot.resolve(moduleName);
   }
 
   private static class HostAndPort {
@@ -81,11 +78,6 @@ public class Util {
     int port;
   }
 
-  @FunctionalInterface
-  public interface SetHost {
-    void setHost(String host, int port) throws IOException;
-  }
-
   @FunctionalInterface
   public interface HandleLink {
     void handle(boolean isEndEffector, String topic, String name) throws IOException;

shared.place/jastadd/types.connect → ros3rag.common/src/main/resources/jastadd/types.connect

@@ -49,3 +49,17 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {:
   }
   return result;
 :}
+
+ParseReachability maps byte[] bytes to de.tudresden.inf.st.ceti.Reachability {:
+  return de.tudresden.inf.st.ceti.Reachability.parseFrom(bytes);
+:}
+
+ConvertReachability maps de.tudresden.inf.st.ceti.Reachability r to CanReachObjectOfInterestWrapper {:
+  var result = new CanReachObjectOfInterestWrapper();
+  for (var objReach : r.getObjectsList()) {
+    if (objReach.getReachable()) {
+      result.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(objReach.getIdObject()));
+    }
+  }
+  return result;
+:}

shared.place/jastadd/types.relast → ros3rag.common/src/main/resources/jastadd/types.relast

@@ -4,6 +4,7 @@ Orientation ::= <X:float> <Y:float> <Z:float> <W:float> ;
 
 Scene ::= DropOffLocation* MovableObject* /LogicalScene/ ;
 
+CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ;
 ObjectOfInterest ::= <Name:String> Position Size Orientation ;
 
 DropOffLocation : ObjectOfInterest ::= ;

ros3rag.placeA/build.gradle

@@ -10,7 +10,14 @@ plugins {
     id 'ros3rag.java-ragconnect-conventions'
 }
 
-mainClassName = 'de.tudresden.inf.st.placeA.SimpleMainA'
+mainClassName = 'de.tudresden.inf.st.placeA.MainA'
+
+task simpleRun(type: JavaExec) {
+    group 'application'
+    classpath sourceSets.main.runtimeClasspath
+    main = "de.tudresden.inf.st.placeA.SimpleMainA"
+
+}
 
 dependencies {
     implementation project(':ros3rag.common')
@@ -23,4 +30,4 @@ ext.ragConnectRootNode = 'WorldModelA'
 ext.relastFiles = ["src/gen/jastadd/WorldModelA.relast", "src/gen/jastadd/RagConnect.relast"]
 ext.jastaddAstPackage = 'de.tudresden.inf.st.placeA.ast'
 
-apply from: '../shared.place/tasks.gradle'
+apply from: '../ros3rag.common/src/main/resources/tasks.gradle'

ros3rag.placeA/src/main/jastadd/WorldModelA.relast

-WorldModelA ::= Robot Scene ; //Task* ;
+WorldModelA ::= Robot [Scene] ; //Task* ;
 
 Robot ::= <Name:String> ;
 

ros3rag.placeA/src/main/jastadd/shared

-../../../../shared.place/jastadd/
\ No newline at end of file
+../../../../ros3rag.common/src/main/resources/jastadd/
\ No newline at end of file

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

 package de.tudresden.inf.st.placeA;
 
 import de.tudresden.inf.st.placeA.ast.MqttHandler;
-import de.tudresden.inf.st.placeA.ast.Position;
+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.DataConfiguration;
 import de.tudresden.inf.st.ros3rag.common.Util;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
 import java.io.File;
-import java.io.IOException;
+import java.nio.charset.StandardCharsets;
 import java.util.concurrent.CountDownLatch;
 import java.util.concurrent.TimeUnit;
 
+import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
+import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
+
 /**
- * TODO: Add description.
+ * Entry point for RAG model in place A.
  *
  * @author rschoene - Initial contribution
  */
 public class MainA {
-
   private static final Logger logger = LogManager.getLogger(MainA.class);
+
+  private final static String TOPIC_MODEL = "place-a/model";
+  private final static String TOPIC_STATUS = "place-a/status";
+  private final static String TOPIC_EXIT = "place-a/exit";
+
+  private final static String TOPIC_SCENE_INIT = "place-a/init";
+  private final static String TOPIC_SCENE_UPDATE_FROM_ROS = "place-a/scene/update";
+  private final static String TOPIC_SCENE_UPDATE_TO_PLACE_B = "place-a/logical/update";
+
   private MqttHandler mainHandler;
   private WorldModelA model;
 
-  private void run(String[] args) throws IOException, InterruptedException {
-    System.out.println("This is place A with " + Position.of(1, 2, 3) + "!");
-    File configFile = new File(args.length == 0 ? "../ros3rag.common/config.yaml" : args[0]);
+  public static void main(String[] args) throws Exception {
+    new MainA().run(args);
+  }
+
+  private void run(String[] args) throws Exception {
+    String configFile = args.length == 0 ? "src/main/resources/config-a.yaml" : args[0];
 
     // --- No configuration below this line ---
 
-    DataConfiguration.ActualConfiguration config = Util.parseConfig(configFile);
+    final var config = Util.parseConfig(UtilA.pathToDirectoryOfPlaceA().resolve(configFile).toFile());
 
     model = new WorldModelA();
 
-    logStatus("Start");
+    /// Prepare main handler
+    mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
+    mainHandler.setHost(config.mqttHost);
     CountDownLatch exitCondition = new CountDownLatch(1);
+    mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
+    mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
+    mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
 
-    logger.info("To print the current model states, send a message to the topic 'model'.");
-    logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C.");
+    /// Reading scene and robot
+    de.tudresden.inf.st.ceti.Scene scene = readScene(
+        UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene)
+    );
+    Scene myScene = UtilA.convert(scene);
+    model.setScene(myScene);
 
-    mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
-    Util.setMqttHost(mainHandler::setHost, config);
-    mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
-    mainHandler.newConnection("exit", bytes -> exitCondition.countDown());
-    mainHandler.newConnection("model", bytes -> logStatus(new String(bytes)));
+    // if multiple robots are specified, then error message will be displayed
+    Util.extractRobotNames(scene).forEach(this::setRobot);
+
+    /// Setup model connection
+    model.ragconnectCheckIncremental();
+    model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
+
+    /// Connect endpoints
+    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);
 
-//    sendInitialDataConfig(mainHandler, config.topics.dataConfig);
+//    mainHandler.publish(mqttUri(TOPIC_SCENE_INIT, config), scene.toByteArray());
+    logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT);
 
     Runtime.getRuntime().addShutdownHook(new Thread(this::close));
 
     exitCondition.await();
+  }
 
-    this.close();
+  private void setRobot(String name) {
+    if (model.getRobot() != null) {
+      logger.error("Robot already set. Overriding with newly found name.");
+    }
+    model.setRobot(new Robot().setName(name));
   }
 
   private void logStatus(String prefix) {
     logger.info(prefix);
-    // more information to be added later
+    String content = UtilA.getModelInfos(model);
+    logger.info("WorldModelA\n{}", content);
+    if (mainHandler != null) {
+      mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8));
+    }
   }
 
   private void close() {
@@ -65,8 +108,4 @@ public class MainA {
     mainHandler.close();
     model.ragconnectCloseConnections();
   }
-
-  public static void main(String[] args) throws IOException, InterruptedException {
-    new MainA().run(args);
-  }
 }

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

 package de.tudresden.inf.st.placeA;
 
-import com.google.protobuf.util.JsonFormat;
 import de.tudresden.inf.st.ceti.Object;
 import de.tudresden.inf.st.placeA.ast.*;
 import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
@@ -8,7 +7,6 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
-import java.nio.file.Files;
 import java.nio.file.Paths;
 import java.util.Scanner;
 import java.util.concurrent.TimeUnit;
@@ -32,9 +30,9 @@ public class SimpleMainA {
 
   @SuppressWarnings("CommentedOutCode")
   private static void testReadSceneConfig() throws Exception {
-    final var path = Paths.get("..", "ros3rag.common", "src", "main", "resources", "config_scene.json");
+    final var path = UtilA.pathToDirectoryOfPlaceA().resolve("src/main/resources/config-scene-a.json");
     de.tudresden.inf.st.ceti.Scene scene = readScene(path);
-    Scene myScene = convert(scene);
+    Scene myScene = UtilA.convert(scene);
 
     var model = new WorldModelA();
     model.setScene(myScene);
@@ -64,16 +62,6 @@ public class SimpleMainA {
     model.getScene().connectLogicalScene("mqtt://localhost/logical/scene/update", true);
   }
 
-  @SuppressWarnings("rawtypes")
-  private static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception {
-    class ExposingASTNode extends ASTNode {
-      public Scene my_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception {
-        return ASTNode._apply_ConvertScene(pbScene);
-      }
-    }
-    return new ExposingASTNode().my_apply_ConvertScene(scene);
-  }
-
   @SuppressWarnings("unused")
   private static void testBuildModelA() {
     WorldModelA model = new WorldModelA();

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.ASTNode;
+import de.tudresden.inf.st.placeA.ast.Scene;
+import de.tudresden.inf.st.placeA.ast.WorldModelA;
+import de.tudresden.inf.st.ros3rag.common.Util;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import java.nio.file.Path;
+
+/**
+ * Static utility methods used only for place A.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class UtilA {
+  private static final Logger logger = LogManager.getLogger(UtilA.class);
+
+  static Path pathToDirectoryOfPlaceA() {
+    return Util.pathToModuleDirectory("ros3rag.placeA");
+  }
+
+  static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception {
+    return new ExposingASTNode().exposed_apply_ConvertScene(scene);
+  }
+
+  public static String getModelInfos(WorldModelA model) {
+    StringBuilder sb = new StringBuilder();
+    sb.append("myRobot: ")
+        .append(model.getRobot().getName())
+        .append("\n");
+    sb.append("myLogicalScene:");
+    if (model.hasScene()) {
+      sb.append("\n").append(model.getScene().getLogicalScene().prettyPrint());
+    } else {
+      sb.append(" (unset)\n");
+    }
+    return sb.toString();
+  }
+
+  @SuppressWarnings("rawtypes")
+  static class ExposingASTNode extends ASTNode {
+    public Scene exposed_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception {
+      return ASTNode._apply_ConvertScene(pbScene);
+    }
+  }
+}

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

+mqttHost: "localhost"
+filenameInitialScene: "src/main/resources/config-scene-a.json"
+useReachability: false

ros3rag.placeB/build.gradle

@@ -11,7 +11,14 @@ plugins {
     id 'ros3rag.java-ragconnect-conventions'
 }
 
-mainClassName = 'de.tudresden.inf.st.placeB.SimpleMainB'
+mainClassName = 'de.tudresden.inf.st.placeB.MainB'
+
+task simpleRun(type: JavaExec) {
+    group 'application'
+    classpath sourceSets.main.runtimeClasspath
+    main = "de.tudresden.inf.st.placeB.SimpleMainB"
+
+}
 
 dependencies {
     implementation project(':ros3rag.common')
@@ -24,4 +31,4 @@ ext.ragConnectRootNode = 'WorldModelB'
 ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/gen/jastadd/RagConnect.relast"]
 ext.jastaddAstPackage = 'de.tudresden.inf.st.placeB.ast'
 
-apply from: '../shared.place/tasks.gradle'
+apply from: '../ros3rag.common/src/main/resources/tasks.gradle'

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

 // --- receiving ---
 receive tree WorldModelB.MyScene using ParseScene, ConvertScene ;
 receive tree WorldModelB.OtherScene ;
+receive tree Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
 
 // --- sending ---
 send tree WorldModelB.NextOperation using PrintOperation ;

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

@@ -74,7 +74,7 @@ aspect Computation {
       // find a location, that can be used as exchange point reached by two robots
       for (var robotForPrevious : robotsFitForPreviousLocation) {
         for (var robotForNew : robotsFitForNewLocation) {
-          for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestList()) {
+          for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
             String objectName = canReachOfRobotForPrevious.getObjectName();
             if (resolveLogicalObjectOfInterest(objectName).isLogicalDropOffLocation() && robotForNew.canReach(objectName)) {
               // add two operations.
@@ -109,7 +109,7 @@ aspect Computation {
 //  syn List<String> locationIds
 
   syn boolean Robot.canReach(String objectName) {
-    for (var canReachObj : getCanReachObjectOfInterestList()) {
+    for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
       if (canReachObj.getObjectName().equals(objectName)) {
         return true;
       }
@@ -169,3 +169,14 @@ aspect Printing {
     return "+Error: " + getErrorMessage() + "+";
   }
 }
+
+aspect Resolving {
+  public Optional<Robot> WorldModelB.findRobot(String name) {
+    for (Robot robot : getRobotList()) {
+      if (robot.getName().equals(name)) {
+        return Optional.of(robot);
+      }
+    }
+    return Optional.empty();
+  }
+}

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

 WorldModelB ::= Robot* [MyScene:Scene] [OtherScene:LogicalScene] /NextOperation:Operation/ ;
 
-Robot ::= <Name:String> CanReachObjectOfInterest* ;
+Robot ::= <Name:String> CanReachObjectOfInterestWrapper ;
 
 abstract Difference ;
 DifferenceObjectAtWrongPlace : Difference ;

ros3rag.placeB/src/main/jastadd/shared

-../../../../shared.place/jastadd/
\ No newline at end of file
+../../../../ros3rag.common/src/main/resources/jastadd/
\ No newline at end of file