From bcd76ebda99d74c540ed4081a3abdbc166d89eb9 Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Mon, 3 May 2021 18:34:32 +0200
Subject: [PATCH] create main entry points, restructure utils
---
.../inf/st/ros3rag/common/Configuration.java | 23 ++++
.../st/ros3rag/common/DataConfiguration.java | 40 -------
.../tudresden/inf/st/ros3rag/common/Util.java | 86 ++++++-------
.../src/main/resources}/jastadd/types.connect | 14 +++
.../src/main/resources}/jastadd/types.jadd | 0
.../src/main/resources}/jastadd/types.relast | 1 +
.../src/main/resources}/tasks.gradle | 0
ros3rag.placeA/build.gradle | 11 +-
.../src/main/jastadd/WorldModelA.relast | 2 +-
ros3rag.placeA/src/main/jastadd/shared | 2 +-
.../de/tudresden/inf/st/placeA/MainA.java | 87 ++++++++++----
.../tudresden/inf/st/placeA/SimpleMainA.java | 16 +--
.../de/tudresden/inf/st/placeA/UtilA.java | 48 ++++++++
.../src/main/resources/config-a.yaml | 3 +
.../src/main/resources/config-scene-a.json | 0
ros3rag.placeB/build.gradle | 11 +-
.../src/main/jastadd/WorldModelB.connect | 1 +
.../src/main/jastadd/WorldModelB.jadd | 15 ++-
.../src/main/jastadd/WorldModelB.relast | 2 +-
ros3rag.placeB/src/main/jastadd/shared | 2 +-
.../de/tudresden/inf/st/placeB/MainB.java | 113 +++++++++++++++++-
.../tudresden/inf/st/placeB/SimpleMainB.java | 111 +++++------------
.../de/tudresden/inf/st/placeB/UtilB.java | 92 ++++++++++++++
.../src/main/resources/config-b.yaml | 8 ++
.../src/main/resources/config-scene-b.json | 21 ++++
.../resources/dummy-reachability-b-arm1.json | 11 ++
.../resources/dummy-reachability-b-arm2.json | 11 ++
27 files changed, 511 insertions(+), 220 deletions(-)
create mode 100644 ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
delete mode 100644 ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java
rename {shared.place => ros3rag.common/src/main/resources}/jastadd/types.connect (74%)
rename {shared.place => ros3rag.common/src/main/resources}/jastadd/types.jadd (100%)
rename {shared.place => ros3rag.common/src/main/resources}/jastadd/types.relast (92%)
rename {shared.place => ros3rag.common/src/main/resources}/tasks.gradle (100%)
create mode 100644 ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
create mode 100644 ros3rag.placeA/src/main/resources/config-a.yaml
rename ros3rag.common/src/main/resources/config_scene.json => ros3rag.placeA/src/main/resources/config-scene-a.json (100%)
create mode 100644 ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
create mode 100644 ros3rag.placeB/src/main/resources/config-b.yaml
create mode 100644 ros3rag.placeB/src/main/resources/config-scene-b.json
create mode 100644 ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json
create mode 100644 ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
new file mode 100644
index 0000000..4323482
--- /dev/null
+++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java
@@ -0,0 +1,23 @@
+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;
+ }
+}
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java
deleted file mode 100644
index 3c4b3d4..0000000
--- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java
+++ /dev/null
@@ -1,40 +0,0 @@
-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;
- }
-}
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java
index fce739f..29c0d85 100644
--- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java
+++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java
@@ -1,19 +1,20 @@
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;
diff --git a/shared.place/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect
similarity index 74%
rename from shared.place/jastadd/types.connect
rename to ros3rag.common/src/main/resources/jastadd/types.connect
index b885ea9..9d2f860 100644
--- a/shared.place/jastadd/types.connect
+++ b/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;
+:}
diff --git a/shared.place/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd
similarity index 100%
rename from shared.place/jastadd/types.jadd
rename to ros3rag.common/src/main/resources/jastadd/types.jadd
diff --git a/shared.place/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast
similarity index 92%
rename from shared.place/jastadd/types.relast
rename to ros3rag.common/src/main/resources/jastadd/types.relast
index aa132b9..c99b527 100644
--- a/shared.place/jastadd/types.relast
+++ b/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 ::= ;
diff --git a/shared.place/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle
similarity index 100%
rename from shared.place/tasks.gradle
rename to ros3rag.common/src/main/resources/tasks.gradle
diff --git a/ros3rag.placeA/build.gradle b/ros3rag.placeA/build.gradle
index c01971a..69f31e9 100644
--- a/ros3rag.placeA/build.gradle
+++ b/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'
diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
index dee022c..67c9b50 100644
--- a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
+++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
@@ -1,4 +1,4 @@
-WorldModelA ::= Robot Scene ; //Task* ;
+WorldModelA ::= Robot [Scene] ; //Task* ;
Robot ::= <Name:String> ;
diff --git a/ros3rag.placeA/src/main/jastadd/shared b/ros3rag.placeA/src/main/jastadd/shared
index 2b0e78b..b6cc005 120000
--- a/ros3rag.placeA/src/main/jastadd/shared
+++ b/ros3rag.placeA/src/main/jastadd/shared
@@ -1 +1 @@
-../../../../shared.place/jastadd/
\ No newline at end of file
+../../../../ros3rag.common/src/main/resources/jastadd/
\ No newline at end of file
diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
index 932100e..83ca62d 100644
--- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
@@ -1,63 +1,106 @@
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");
-// sendInitialDataConfig(mainHandler, config.topics.dataConfig);
+ 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);
+
+// 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);
- }
}
diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java
index b26dc9a..327b5b5 100644
--- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java
@@ -1,6 +1,5 @@
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();
diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
new file mode 100644
index 0000000..c791077
--- /dev/null
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java
@@ -0,0 +1,48 @@
+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);
+ }
+ }
+}
diff --git a/ros3rag.placeA/src/main/resources/config-a.yaml b/ros3rag.placeA/src/main/resources/config-a.yaml
new file mode 100644
index 0000000..a3901c5
--- /dev/null
+++ b/ros3rag.placeA/src/main/resources/config-a.yaml
@@ -0,0 +1,3 @@
+mqttHost: "localhost"
+filenameInitialScene: "src/main/resources/config-scene-a.json"
+useReachability: false
diff --git a/ros3rag.common/src/main/resources/config_scene.json b/ros3rag.placeA/src/main/resources/config-scene-a.json
similarity index 100%
rename from ros3rag.common/src/main/resources/config_scene.json
rename to ros3rag.placeA/src/main/resources/config-scene-a.json
diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle
index 5060052..95d2d16 100644
--- a/ros3rag.placeB/build.gradle
+++ b/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'
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index a346406..f773747 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -1,6 +1,7 @@
// --- 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 ;
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index 2dbc757..047ade0 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/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();
+ }
+}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index e6941e4..6955828 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
@@ -1,6 +1,6 @@
WorldModelB ::= Robot* [MyScene:Scene] [OtherScene:LogicalScene] /NextOperation:Operation/ ;
-Robot ::= <Name:String> CanReachObjectOfInterest* ;
+Robot ::= <Name:String> CanReachObjectOfInterestWrapper ;
abstract Difference ;
DifferenceObjectAtWrongPlace : Difference ;
diff --git a/ros3rag.placeB/src/main/jastadd/shared b/ros3rag.placeB/src/main/jastadd/shared
index 2b0e78b..b6cc005 120000
--- a/ros3rag.placeB/src/main/jastadd/shared
+++ b/ros3rag.placeB/src/main/jastadd/shared
@@ -1 +1 @@
-../../../../shared.place/jastadd/
\ No newline at end of file
+../../../../ros3rag.common/src/main/resources/jastadd/
\ No newline at end of file
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
index 02bffb3..5548372 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java
@@ -1,12 +1,119 @@
package de.tudresden.inf.st.placeB;
+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.Util;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+
+import java.io.File;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+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 B.
*
* @author rschoene - Initial contribution
*/
public class MainB {
- public static void main(String[] args) {
- System.out.println("This is another place B!");
+ private static final Logger logger = LogManager.getLogger(SimpleMainB.class);
+
+ private final static String TOPIC_MODEL = "place-b/model";
+ private final static String TOPIC_STATUS = "place-b/status";
+ private final static String TOPIC_EXIT = "place-b/exit";
+
+ private final static String TOPIC_MY_SCENE_INIT = "place-b/init";
+ private final static String TOPIC_MY_SCENE_UPDATE_FROM_ROS = "place-b/scene/update";
+ private final static String TOPIC_COMMANDS = "place-b/commands";
+
+ private final static String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A = "place-a/logical/update";
+
+ private MqttHandler mainHandler;
+ private WorldModelB model;
+
+ public static void main(String[] args) throws Exception {
+ new MainB().run(args);
}
+
+ private void run(String[] args) throws Exception {
+ String configFile = args.length == 0 ? "src/main/resources/config-b.yaml" : args[0];
+
+ // --- No configuration below this line ---
+
+ final var config = Util.parseConfig(UtilB.pathToDirectoryOfPlaceB().resolve(configFile).toFile());
+ model = new WorldModelB();
+
+ /// Prepare main handler
+ mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
+ mainHandler.setHost(config.mqttHost);
+ mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
+ CountDownLatch exitCondition = new CountDownLatch(1);
+ mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
+ mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
+
+ /// Reading scene and robots
+ de.tudresden.inf.st.ceti.Scene scene = readScene(
+ UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene)
+ );
+ Scene myScene = UtilB.convert(scene);
+ model.setMyScene(myScene);
+ Util.extractRobotNames(scene).forEach(name -> model.addRobot(UtilB.createRobot(name)));
+
+ /// Set (dummy) reachability, if configured
+ if (config.useReachability) {
+ for (Configuration.ReachabilityConfig reachabilityConfig : config.reachability) {
+ Robot robot = model.findRobot(reachabilityConfig.idRobot).orElseThrow(
+ () -> new IllegalArgumentException("Could not find robot with name " + reachabilityConfig.idRobot)
+ );
+ Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename));
+ Reachability reachability = UtilB.readReachability(path);
+ CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
+ robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
+ }
+ }
+
+ /// Setup model connection
+ model.ragconnectCheckIncremental();
+ model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
+
+ /// Connect endpoints
+ model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config));
+ model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config));
+ model.connectNextOperation(mqttUri(TOPIC_COMMANDS, 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);
+
+// mainHandler.publish(mqttUri(TOPIC_SCENE_INIT, config), 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) {
+ logger.info(message);
+ String content = UtilB.getModelInfos(model);
+ logger.info("WorldModelB\n{}", content);
+ if (mainHandler != null) {
+ mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8));
+ }
+ }
+
+ private void close() {
+ logger.info("Exiting ...");
+ mainHandler.close();
+ model.ragconnectCloseConnections();
+ }
+
}
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
index d00c6a2..20b29d2 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
@@ -1,15 +1,17 @@
package de.tudresden.inf.st.placeB;
+import com.google.protobuf.util.JsonFormat;
+import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ast.*;
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.Files;
+import java.nio.file.Path;
import java.nio.file.Paths;
-import java.util.concurrent.CountDownLatch;
-import java.util.concurrent.TimeUnit;
-
-import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
/**
* Testing features for placeB.
@@ -18,83 +20,44 @@ import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
*/
public class SimpleMainB {
private static final Logger logger = LogManager.getLogger(SimpleMainB.class);
- private MqttHandler mainHandler;
- private WorldModelB model;
public static void main(String[] args) throws Exception {
+ System.out.println("Running SimpleMainB");
new SimpleMainB().run(args);
}
+ @SuppressWarnings({"unused", "CommentedOutCode"})
private void run(String[] args) throws Exception {
// testBuildModelB();
- testReceivingModelB();
+// testReceivingModelB();
+ testReachability();
}
- private void testReceivingModelB() throws Exception {
- final var path = Paths.get("..", "ros3rag.common", "src", "main", "resources", "config_scene.json");
-
- model = new WorldModelB();
-
- de.tudresden.inf.st.ceti.Scene scene = readScene(path);
- Scene myScene = convert(scene);
- model.setMyScene(myScene);
-
- model.ragconnectCheckIncremental();
- model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
-
- model.connectOtherScene("mqtt://localhost/logical/scene/update");
-
- logStatus("Start");
- CountDownLatch exitCondition = new CountDownLatch(1);
-
- 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.");
-
- 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)));
-
-// sendInitialDataConfig(mainHandler, config.topics.dataConfig);
-
- Runtime.getRuntime().addShutdownHook(new Thread(this::close));
-
- exitCondition.await();
-
- this.close();
- }
-
- private void logStatus(String message) {
- logger.info(message);
- // more information to be added later
+ private void testReachability() throws Exception {
+ WorldModelB model = new WorldModelB();
+ Robot arm1 = UtilB.createRobot("ARM1");
+ model.addRobot(arm1);
+ Robot arm2 = UtilB.createRobot("ARM2");
+ model.addRobot(arm2);
+
+ Reachability reachabilityArm1 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB()
+ .resolve("src/main/resources/dummy-reachability-b-arm1.json"));
+ Reachability reachabilityArm2 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB()
+ .resolve("src/main/resources/dummy-reachability-b-arm2.json"));
+ CanReachObjectOfInterestWrapper canReachForArm1 = UtilB.convert(reachabilityArm1);
+ CanReachObjectOfInterestWrapper canReachForArm2 = UtilB.convert(reachabilityArm2);
+ arm1.setCanReachObjectOfInterestWrapper(canReachForArm1);
+ arm2.setCanReachObjectOfInterestWrapper(canReachForArm2);
printModelInfos(model);
}
- private void close() {
- logger.info("Exiting ...");
- mainHandler.close();
- model.ragconnectCloseConnections();
- }
-
-
- @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 void testBuildModelB() {
WorldModelB model = new WorldModelB();
// robots
- model.addRobot(createRobot("Alice", "placeAlfa", "placeBeta"));
- model.addRobot(createRobot("Bob", "placeBeta", "placeGamma"));
+ model.addRobot(UtilB.createRobot("Alice", "placeAlfa", "placeBeta"));
+ model.addRobot(UtilB.createRobot("Bob", "placeBeta", "placeGamma"));
// myScene
var myScene = new Scene();
@@ -143,23 +106,7 @@ public class SimpleMainB {
printModelInfos(model);
}
- private static void printModelInfos(WorldModelB model) {
- logger.info("myRobots: {}", model.getRobotList().prettyPrint(
- robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestList().prettyPrint(CanReachObjectOfInterest::getObjectName) + ")"
- ));
- logger.info("myLogicalScene:\n{}", model.getMyScene().getLogicalScene().prettyPrint());
- logger.info("otherScene:\n{}", model.getOtherScene().prettyPrint());
- logger.info("Diff: {}", model.diffScenes().prettyPrint());
- logger.info("Operations: {}", model.diffToOperations().prettyPrint());
- logger.info("Next operation: {}", model.getNextOperation());
- }
-
- private static Robot createRobot(String name, String... canReach) {
- Robot result = new Robot()
- .setName(name);
- for (String canReachName : canReach) {
- result.addCanReachObjectOfInterest(new CanReachObjectOfInterest(canReachName));
- }
- return result;
+ private void printModelInfos(WorldModelB model) {
+ logger.info(UtilB.getModelInfos(model));
}
}
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
new file mode 100644
index 0000000..80b67ac
--- /dev/null
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java
@@ -0,0 +1,92 @@
+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.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.Files;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Static utility methods used only for place B.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class UtilB {
+ private static final Logger logger = LogManager.getLogger(UtilB.class);
+
+ static Reachability readReachability(Path path) throws IOException {
+ logger.debug("Reading reachability from {}", path.toAbsolutePath());
+ var jsonString = Files.readString(path);
+ var builder = de.tudresden.inf.st.ceti.Reachability.newBuilder();
+ JsonFormat.parser().merge(jsonString, builder);
+ return builder.build();
+ }
+
+ static Path pathToDirectoryOfPlaceB() {
+ return Util.pathToModuleDirectory("ros3rag.placeB");
+ }
+
+ static Robot createRobot(String name, String... canReach) {
+ Robot result = new Robot().setName(name);
+ CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper();
+
+ for (String canReachName : canReach) {
+ wrapper.addCanReachObjectOfInterest(new CanReachObjectOfInterest(canReachName));
+ }
+ result.setCanReachObjectOfInterestWrapper(wrapper);
+ return result;
+ }
+
+ static String getModelInfos(WorldModelB model) {
+ StringBuilder sb = new StringBuilder();
+ sb.append("myRobots: ")
+ .append(model.getRobotList().prettyPrint(
+ robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList().prettyPrint(CanReachObjectOfInterest::getObjectName) + ")"))
+ .append("\n");
+ sb.append("myLogicalScene:");
+ if (model.hasMyScene()) {
+ sb.append("\n").append(model.getMyScene().getLogicalScene().prettyPrint());
+ } else {
+ sb.append(" (unset)\n");
+ }
+ sb.append("otherScene:");
+ if (model.hasOtherScene()) {
+ sb.append("\n").append(model.getOtherScene().prettyPrint());
+ } else {
+ sb.append(" (unset)\n");
+ }
+ sb.append("Diff: ").append(model.diffScenes().prettyPrint()).append("\n");
+ sb.append("Operations: ").append(model.diffToOperations().prettyPrint()).append("\n");
+ sb.append("Next operation: ").append(model.getNextOperation()).append("\n");
+ return sb.toString();
+ }
+
+ static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception {
+ return new ExposingASTNode().exposed_apply_ConvertScene(scene);
+ }
+
+ static CanReachObjectOfInterestWrapper convert(de.tudresden.inf.st.ceti.Reachability r) throws Exception {
+ return new ExposingASTNode().exposed_apply_ConvertReachability(r);
+ }
+
+ @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);
+ }
+ public CanReachObjectOfInterestWrapper exposed_apply_ConvertReachability(de.tudresden.inf.st.ceti.Reachability r) throws Exception {
+ return ASTNode._apply_ConvertReachability(r);
+ }
+ }
+
+}
diff --git a/ros3rag.placeB/src/main/resources/config-b.yaml b/ros3rag.placeB/src/main/resources/config-b.yaml
new file mode 100644
index 0000000..91aa941
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/config-b.yaml
@@ -0,0 +1,8 @@
+mqttHost: "localhost"
+filenameInitialScene: "src/main/resources/config-scene-b.json"
+useReachability: true
+reachability:
+ - idRobot: "arm1"
+ filename: "src/main/resources/dummy-reachability-b-arm1.json"
+ - idRobot: "arm2"
+ filename: "src/main/resources/dummy-reachability-b-arm2.json"
diff --git a/ros3rag.placeB/src/main/resources/config-scene-b.json b/ros3rag.placeB/src/main/resources/config-scene-b.json
new file mode 100644
index 0000000..f14e17f
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/config-scene-b.json
@@ -0,0 +1,21 @@
+{ "objects": [
+ { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
+ { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
+ { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
+ { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
+ { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } },
+ { "id": "binBlue","type": "BIN","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } },
+ { "id": "binRed","type": "BIN","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } },
+ { "id": "binGreen","type": "BIN","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } },
+ { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
+ { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } },
+ { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } },
+ { "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
+ { "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 1,"w": 0.92388 },"color": { "g": 1 } },
+ { "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } },
+ { "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
+ { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
+ { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } },
+ { "id": "arm1","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } },
+ { "id": "arm2","type": "ARM","pos": { "x": 1, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
+] }
diff --git a/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json
new file mode 100644
index 0000000..d238077
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json
@@ -0,0 +1,11 @@
+{
+ "idRobot": "ARM1",
+ "objects": [
+ { "idObject": "objectRed1", "reachable": true },
+ { "idObject": "objectRed2", "reachable": true },
+ { "idObject": "objectRed3", "reachable": false },
+ { "idObject": "binRed", "reachable": true },
+ { "idObject": "binBlue", "reachable": true },
+ { "idObject": "binGreen", "reachable": false }
+ ]
+}
diff --git a/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json
new file mode 100644
index 0000000..c058487
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json
@@ -0,0 +1,11 @@
+{
+ "idRobot": "ARM2",
+ "objects": [
+ { "idObject": "objectRed1", "reachable": false },
+ { "idObject": "objectRed2", "reachable": false },
+ { "idObject": "objectRed3", "reachable": false },
+ { "idObject": "binRed", "reachable": false },
+ { "idObject": "binBlue", "reachable": true },
+ { "idObject": "binGreen", "reachable": true }
+ ]
+}
--
GitLab