diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000000000000000000000000000000000000..b90b9d73797f9f1e8360876dc6a9e62f3f541ae3
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,8 @@
+build/
+.idea/
+.gradle/
+.git/
+ros3rag.placeA/build/
+ros3rag.placeB/build/
+ros3rag.common/build/
+Dockerfile
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 15774ae91cb09f92b308577d34b1f9058bb708f5..f3c7e98935ba157aecac5bf3d4343db4b0a2f411 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -8,4 +8,4 @@ build:
image: openjdk:11
stage: build
script:
- - ./gradlew assemble
+ - ./gradlew build
diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 0000000000000000000000000000000000000000..b87f50be67c42d8f326c6f5d13117f55f73af32e
--- /dev/null
+++ b/Dockerfile
@@ -0,0 +1,27 @@
+FROM openjdk:11
+
+ENV DEBIAN_FRONTEND noninteractive
+ENV HOME /home/user
+RUN useradd --create-home --home-dir $HOME user \
+ && chmod -R u+rwx $HOME \
+ && chown -R user:user $HOME
+
+USER user
+
+ENV GRADLE_USER_HOME /home/user/.gradle
+COPY --chown=user:user gradle /temp/gradle
+COPY --chown=user:user gradlew /temp/
+
+WORKDIR /temp
+RUN ./gradlew --no-daemon --version
+
+COPY --chown=user:user . /ros3rag/
+
+WORKDIR /ros3rag
+
+RUN ./gradlew --no-daemon installDist
+
+# target is either: ros3rag.placeA oder ros3rag.placeB
+ENV TARGET unspecified
+# ENV CONFIG_FILE ./ros3rag.common/config.yaml
+ENTRYPOINT ["/bin/bash", "-c", "./${TARGET}/build/install/${TARGET}/bin/${TARGET}"]
diff --git a/buildSrc/src/main/groovy/ros3rag.java-application-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-application-conventions.gradle
index ca778a499ace2bc60db88434e4f64745d05b4717..2242c13850ef41b8c202298aeab8ee06fded6cde 100644
--- a/buildSrc/src/main/groovy/ros3rag.java-application-conventions.gradle
+++ b/buildSrc/src/main/groovy/ros3rag.java-application-conventions.gradle
@@ -2,7 +2,3 @@ plugins {
id 'ros3rag.java-common-conventions'
id 'application'
}
-
-run {
- standardInput = System.in
-}
diff --git a/buildSrc/src/main/groovy/ros3rag.java-jastadd-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-jastadd-conventions.gradle
index a2eada848ea2ee77f105a56295102f64f5b65430..9549924c8d8e4928d9989ae5d61929d3dd2285ab 100644
--- a/buildSrc/src/main/groovy/ros3rag.java-jastadd-conventions.gradle
+++ b/buildSrc/src/main/groovy/ros3rag.java-jastadd-conventions.gradle
@@ -2,6 +2,7 @@ plugins {
id 'ros3rag.java-common-conventions'
id 'java-library'
id 'idea'
+ id 'org.jastadd'
}
dependencies {
diff --git a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle
index 2281b3436d3e0b1ba6c88335e6f366642143ab07..ad1b6a77d40560af9619a15e0791845b4a506306 100644
--- a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle
+++ b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle
@@ -3,18 +3,19 @@ plugins {
}
configurations {
- baseRuntimeClasspath
- ragconnectClasspath
- grammar2umlClasspath
+ ragconnect
+ grammar2uml
+ relast
}
dependencies {
api group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11'
- grammar2umlClasspath group: 'de.tudresden.inf.st', name: 'grammar2uml', version: '0.1.1'
+ grammar2uml group: 'de.tudresden.inf.st', name: 'grammar2uml', version: '0.2.3'
- ragconnectClasspath group: 'de.tudresden.inf.st', name: 'ragconnect', version: '0.3.0'
-// ragconnectClasspath fileTree(include: ['ragconnect-0.2.5.jar'], dir: '../libs')
+ relast group: 'org.jastadd', name: 'relast', version: "0.4.0-143"
+ ragconnect group: 'de.tudresden.inf.st', name: 'ragconnect', version: '1.0.0-alpha-198'
// implementation group: 'de.tudresden.inf.st', name: 'dumpAstWithPlantuml', version: '0.3.5'
// implementation fileTree(include: ['dumpAstWithPlantuml-0.3.5.jar'], dir: '../libs')
}
+
diff --git a/cloc/run-cloc.sh b/cloc/run-cloc.sh
index 15422cde3c5d2fa2a1f84cf4487f72ce89380531..56f3d936b2104e98beb324036d90630975fcae53 100755
--- a/cloc/run-cloc.sh
+++ b/cloc/run-cloc.sh
@@ -6,6 +6,9 @@ then
:
else
+# remove previous results
+rm *-result.txt
+
# --force-lang=Java,jrag --force-lang=Java,jadd
DEF_FILE=my_definitions.txt
echo "Export language definitions"
@@ -17,11 +20,14 @@ done
echo "Running cloc with new definitions"
# --ignored=bad-files.txt
CLOC_CMD="cloc --exclude-lang=JSON --read-lang-def=my_definitions.txt --exclude-list-file=.clocignore --quiet"
-$CLOC_CMD --report-file=place-result.txt ../ros3rag.place?/src/main/ja* 2>>cloc-errors.log
-$CLOC_CMD --report-file=common-result.txt ../ros3rag.common/src/main 2>>cloc-errors.log
-
-$CLOC_CMD --report-file=place-gen-result.txt ../ros3rag.place?/src/gen 2>>cloc-errors.log
-$CLOC_CMD --report-file=common-gen-result.txt ../ros3rag.common/build/generated/source/proto/main 2>>cloc-errors.log
+$CLOC_CMD --report-file=common-result.txt ../ros3rag.common/src/main/resources/jastadd ../ros3rag.common/src/main/java 2>>cloc-errors.log
+$CLOC_CMD --report-file=placeA-result.txt ../ros3rag.placeA/src/main/ja* 2>>cloc-errors.log
+$CLOC_CMD --report-file=placeB-result.txt ../ros3rag.placeB/src/main/ja* 2>>cloc-errors.log
+
+# $CLOC_CMD --report-file=base-gen-result.txt ../ros3rag.base/src/gen 2>>cloc-errors.log
+$CLOC_CMD --report-file=placeA-gen-result.txt ../ros3rag.placeA/src/gen 2>>cloc-errors.log
+$CLOC_CMD --report-file=placeB-gen-result.txt ../ros3rag.placeB/src/gen 2>>cloc-errors.log
+$CLOC_CMD --report-file=common-gen-result.txt ../ros3rag.common/build/generated/source/proto/main/java/de 2>>cloc-errors.log
# CFC_CMD='grep -o 'if'\|'for'\|'return''
diff --git a/gradle.properties b/gradle.properties
index 054aadd59669403c2259cbc5cef9748baef94bce..45378cc37ed4a3ac3643f8748530f9082394c660 100644
--- a/gradle.properties
+++ b/gradle.properties
@@ -1,6 +1,8 @@
-jackson_version = 2.12.3
-jupiter_version = 5.8.0-M1
-assertj_version = 3.19.0
-apache_httpcomponents_version = 4.5.8
-log4j_version = 2.14.1
+jackson_version = 2.13.2.2
+jackson_yaml_version = 2.13.2
+jupiter_version = 5.8.2
+assertj_version = 3.22.0
+log4j_version = 2.17.2
protobuf_version = 4.0.0-rc-2
+protobuf_plugin_version = 0.8.18
+jastadd_gradle_version = 1.14.5
diff --git a/libs/jastadd2.jar b/libs/jastadd2.jar
new file mode 100644
index 0000000000000000000000000000000000000000..d615b895453d660f0e7397fffad58a05029169fd
Binary files /dev/null and b/libs/jastadd2.jar differ
diff --git a/ros3rag.common/build.gradle b/ros3rag.common/build.gradle
index 40c0c7ff928270f9147e88caec466b904cf64815..d3efb0c5ed12f6d9609eb725358a3f3ebca5c707 100644
--- a/ros3rag.common/build.gradle
+++ b/ros3rag.common/build.gradle
@@ -1,19 +1,18 @@
buildscript {
- repositories.jcenter()
+ repositories.mavenCentral()
dependencies {
- classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.16'
+ classpath "com.google.protobuf:protobuf-gradle-plugin:${protobuf_plugin_version}"
}
}
plugins {
id 'ros3rag.java-application-conventions'
id 'java-library'
+ id "com.google.protobuf" version "${protobuf_plugin_version}"
}
-apply plugin: 'com.google.protobuf'
-
dependencies {
- implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_version}"
+ implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_yaml_version}"
api group: 'com.google.protobuf', name: 'protobuf-java', version: "${protobuf_version}"
api group: 'com.google.protobuf', name: 'protobuf-java-util', version: "${protobuf_version}"
api group: 'org.apache.commons', name: 'commons-math3', version: '3.6.1'
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
index 4323482606e8a6850c3304bb4f90fb3ff2919fb7..962e31ac5a8a74840a9985b5cd71c9b9026166af 100644
--- 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
@@ -14,10 +14,15 @@ public class Configuration {
public String mqttHost;
public String filenameInitialScene;
public boolean useReachability;
+ public String coordinatorMqttTopicPrefix;
public List<ReachabilityConfig> reachability;
public static class ReachabilityConfig {
public String idRobot;
public String filename;
}
+
+ public boolean useCoordinator() {
+ return coordinatorMqttTopicPrefix != null;
+ }
}
diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java
new file mode 100644
index 0000000000000000000000000000000000000000..dfb03f3f94b8c06192e181b52f23713d597b8406
--- /dev/null
+++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java
@@ -0,0 +1,206 @@
+package de.tudresden.inf.st.ros3rag.common;
+
+import de.tudresden.inf.st.ceti.Scene;
+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.util.LinkedHashMap;
+import java.util.Map;
+import java.util.concurrent.CountDownLatch;
+import java.util.concurrent.TimeUnit;
+import java.util.function.Consumer;
+
+/**
+ * Shared parts of both main classes.
+ *
+ * @author rschoene - Initial contribution
+ */
+public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHandlerWrapper<MqttHandler>,
+ WorldModel extends SharedMainParts.WorldModelWrapper> {
+ protected final Logger logger = LogManager.getLogger(this.getClass());
+
+ private final String TOPIC_MODEL;
+ private final String TOPIC_STATUS;
+ private final String TOPIC_REWIND;
+ private final String TOPIC_EXIT;
+ private final String TOPIC_SCENE_INIT;
+
+ private static final String TOPIC_SUFFIX_COORDINATOR_STATUS = "status";
+ private static final String TOPIC_SUFFIX_COORDINATOR_COMMAND = "command";
+
+ protected MqttHandler mainHandler;
+ protected WorldModel model;
+ protected Configuration config;
+ protected final String cellName;
+ protected final Path pathToConfig;
+ private CountDownLatch startCondition;
+
+ public SharedMainParts(String cellName, Path pathToConfig) {
+ this.cellName = cellName;
+ this.pathToConfig = pathToConfig;
+
+ this.TOPIC_MODEL = cellName + "/model";
+ this.TOPIC_STATUS = cellName + "/status";
+ this.TOPIC_REWIND = cellName + "/rewind";
+ this.TOPIC_EXIT = cellName + "/exit";
+
+ this.TOPIC_SCENE_INIT = cellName + "/scene/init";
+ }
+
+ protected abstract MqttHandler createMqttHandler();
+
+ private Map<String, String> channelDescriptions() {
+ return 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 void run() throws Exception {
+ config = Util.parseConfig(pathToConfig.toFile());
+
+ /// Prepare main handler
+ mainHandler = createMqttHandler().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)));
+ mainHandler.newConnection(TOPIC_REWIND, bytes ->
+ {
+ try {
+ rewind("rewind");
+ } catch (Exception e) {
+ logger.catching(e);
+ }
+ }
+ );
+ if (config.useCoordinator()) {
+ logger.info("Using coordinator logic");
+ mainHandler.newConnection(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_COMMAND),
+ bytes -> reactToCoordinatorCommand(new String(bytes)));
+ }
+
+ createSpecificMainHandlerConnections();
+
+ logger.info("Supported commands: {}", channelDescriptions());
+
+ rewind("Start");
+
+ Runtime.getRuntime().addShutdownHook(new Thread(this::close));
+
+ exitCondition.await();
+ }
+
+ private String joinTopics(String... topics) {
+ StringBuilder sb = new StringBuilder();
+ for (int i = 0; i < topics.length; i++) {
+ String topic = topics[i];
+ if (i > 0 && topic.startsWith("/")) {
+ topic = topic.substring(1);
+ }
+ if (i < topics.length - 1 && topic.endsWith("/")) {
+ topic = topic.substring(0, topic.length() - 1);
+ }
+ if (i > 0) {
+ sb.append("/");
+ }
+ sb.append(topic);
+ }
+ return sb.toString();
+ }
+
+ private void reactToCoordinatorCommand(String command) {
+ switch (command) {
+ case "rewind":
+ try {
+ rewind(command);
+ } catch (Exception e) {
+ e.printStackTrace();
+ }
+ break;
+ case "start":
+ startCondition.countDown();
+ break;
+ default:
+ System.err.println("Unknown coordinator command: " + command);
+ }
+ }
+
+ protected abstract void createSpecificMainHandlerConnections();
+
+ private void rewind(String statusMessage) throws Exception {
+ if (model != null) {
+ model.ragconnectCloseConnections();
+ }
+ model = createWorldModel();
+
+ Scene scene = readSceneAndRobots();
+
+ /// Setup model connection
+ model.ragconnectCheckIncremental();
+ model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
+
+ if (config.useCoordinator()) {
+ startCondition = new CountDownLatch(1);
+ mainHandler.publish(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_STATUS),
+ "up".getBytes(StandardCharsets.UTF_8));
+ startCondition.await();
+ }
+
+ connectEndpoints();
+
+ logStatus(statusMessage);
+
+ mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray());
+ if (config.useCoordinator()) {
+ mainHandler.publish(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_STATUS),
+ "ready".getBytes(StandardCharsets.UTF_8));
+ }
+ }
+
+ protected abstract Scene readSceneAndRobots() throws Exception;
+
+ protected abstract void connectEndpoints() throws IOException;
+
+ protected abstract WorldModel createWorldModel();
+
+ protected abstract String getModelInfos(WorldModel model, boolean detailed);
+
+ private void logStatus(String message) {
+ logger.info(message);
+ String content = getModelInfos(model, message.equals("Start") || message.startsWith("detail"));
+ 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();
+ }
+
+ public interface MqttHandlerWrapper<SELF> {
+ SELF dontSendWelcomeMessage();
+ boolean newConnection(String topic, Consumer<byte[]> callback);
+ void publish(String topic, byte[] payload);
+ void close();
+
+ SELF setHost(String host) throws java.io.IOException;
+
+ @SuppressWarnings("UnusedReturnValue")
+ boolean waitUntilReady(long value, TimeUnit unit);
+ }
+
+ public interface WorldModelWrapper {
+ void ragconnectCheckIncremental();
+ void ragconnectSetupMqttWaitUntilReady(long value, TimeUnit unit);
+ void ragconnectCloseConnections();
+ }
+}
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 29c0d850fd2c370f5776469701314c9ac7c5c0d5..1b8b116cf7dd075207d3b3177f619b543841a814 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
@@ -14,7 +14,9 @@ import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
+import java.util.HashSet;
import java.util.List;
+import java.util.Set;
/**
* Helper method dealing with config.
@@ -58,11 +60,18 @@ public class Util {
return pathToModuleDirectory("ros3rag.common");
}
+ private final static Set<String> modules = new HashSet<>() {{
+ add("ros3rag.placeA");
+ add("ros3rag.placeB");
+ add("ros3rag.common");
+ add("ros3rag.coordinator");
+ }};
+
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")) {
+ if (modules.contains(currentFileName)) {
// we are in some module, use parent
repoRoot = current.getParent();
} else if (current.resolve(moduleName).toFile().exists()) {
@@ -72,14 +81,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;
- }
}
diff --git a/ros3rag.common/src/main/proto/cgv_connector.proto b/ros3rag.common/src/main/proto/cgv_connector.proto
index be68b973f8c82c5b99b10e7fefcbbf4c05bb4c5d..ca2f8c48e61b1d433d5322894dac851cfb6491af 100644
--- a/ros3rag.common/src/main/proto/cgv_connector.proto
+++ b/ros3rag.common/src/main/proto/cgv_connector.proto
@@ -37,6 +37,7 @@ message Object {
BOX = 1;
BIN = 2;
ARM = 3;
+ DROP_OFF_LOCATION = 4;
}
string id = 1;
@@ -72,4 +73,4 @@ message Reachability {
}
string idRobot = 1; // the id of the robot arm
repeated ObjectReachability objects = 2; // all objects reachable
-}
+}
\ No newline at end of file
diff --git a/ros3rag.common/src/main/resources/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect
index 9d2f860d5e7e2c3e2457a00d54fa763756137ecc..ed12a0066808358bff82d26c5a8630e3b35436e1 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.connect
+++ b/ros3rag.common/src/main/resources/jastadd/types.connect
@@ -13,8 +13,8 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {:
result.addMovableObject(movableObject);
obj = movableObject;
break;
- case BIN:
- // BIN == DropOffLocation
+ case DROP_OFF_LOCATION:
+ // DROP_OFF_LOCATION == DropOffLocation
var dropOffLocation = new DropOffLocation();
result.addDropOffLocation(dropOffLocation);
obj = dropOffLocation;
diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd
index edaf5b36248eb2c642e81ff299a83a54dfaf28f6..0d9fe88eb118fec13f1aadec7f72440fbefab760 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.jadd
+++ b/ros3rag.common/src/main/resources/jastadd/types.jadd
@@ -2,33 +2,8 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import java.util.*;
-aspect GrammarTypes {
- public static Position Position.of(int x, int y, int z) {
- return new Position(x, y, z);
- }
-
- @Override
- public boolean Position.equals(Object o) {
- if (this == o) return true;
- if (o == null || getClass() != o.getClass()) return false;
- Position that = (Position) o;
- return getX() == that.getX() &&
- getY() == that.getY() &&
- getZ() == that.getZ();
- }
-
- @Override
- public int Position.hashCode() {
- return java.util.Objects.hash(getX(), getY(), getZ());
- }
-
- @Override
- public String Position.toString() {
- return "(" + getX() + ", " + getY() + ", " + getZ() + ")";
- }
-}
-
aspect Resolving {
+ //--- resolveObjectOfInterest ---
syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) {
for (DropOffLocation location : getDropOffLocationList()) {
if (location.getName().equals(name)) {
@@ -42,6 +17,8 @@ aspect Resolving {
}
return null;
}
+
+ //--- resolveLogicalObjectOfInterest ---
syn LogicalObjectOfInterest LogicalScene.resolveLogicalObjectOfInterest(String name) {
for (LogicalDropOffLocation location : getLogicalDropOffLocationList()) {
if (location.getName().equals(name)) {
@@ -58,6 +35,7 @@ aspect Resolving {
}
aspect Computation {
+ //--- isLocatedAt ---
syn boolean MovableObject.isLocatedAt(DropOffLocation location) {
Orientation orient = location.getOrientation();
Position locationPosition = location.getPosition();
@@ -84,34 +62,35 @@ aspect Computation {
double rotatedY = rotatedObjectVector.getY();
double rotatedZ = rotatedObjectVector.getZ();
- System.out.printf("min: (%f , %f , %f). rotated: (%f , %f , %f), max: (%f , %f , %f)%n",
- -halfLocationLength, -halfLocationWidth, -halfLocationHeight,
- rotatedX, rotatedY, rotatedZ,
- halfLocationLength,
- halfLocationWidth,
- halfLocationHeight);
+// System.out.printf("min: (%f , %f , %f). rotated: (%f , %f , %f), max: (%f , %f , %f)%n",
+// -halfLocationLength, -halfLocationWidth, -halfLocationHeight,
+// rotatedX, rotatedY, rotatedZ,
+// halfLocationLength,
+// halfLocationWidth,
+// halfLocationHeight);
return LT(-halfLocationLength, rotatedX) && LT(rotatedX, halfLocationLength) &&
LT(-halfLocationWidth, rotatedY) && LT(rotatedY, halfLocationWidth) &&
LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight);
}
+ //--- getLogicalScene ---
syn LogicalScene Scene.getLogicalScene() {
var result = new LogicalScene();
Map<MovableObject, LogicalMovableObject> objects = new HashMap<>();
+ for (MovableObject movableObject : getMovableObjectList()) {
+ var newLogicalMovableObject = new LogicalMovableObject();
+ newLogicalMovableObject.setName(movableObject.getName());
+ result.addLogicalMovableObject(newLogicalMovableObject);
+ objects.put(movableObject, newLogicalMovableObject);
+ }
for (DropOffLocation location : getDropOffLocationList()) {
var logicalLocation = new LogicalDropOffLocation();
logicalLocation.setName(location.getName());
result.addLogicalDropOffLocation(logicalLocation);
for (MovableObject movableObject : getMovableObjectList()) {
if (movableObject.isLocatedAt(location)) {
- LogicalMovableObject logicalMovableObject = objects.computeIfAbsent(movableObject, k -> {
- var newLogicalMovableObject = new LogicalMovableObject();
- newLogicalMovableObject.setName(k.getName());
- result.addLogicalMovableObject(newLogicalMovableObject);
- return newLogicalMovableObject;
- });
- logicalLocation.addContainedObject(logicalMovableObject);
+ logicalLocation.addContainedObject(objects.get(movableObject));
}
}
}
@@ -120,6 +99,7 @@ aspect Computation {
private static final double MovableObject.DELTA = 0.0001;
+ //--- LT ---
/**
* @return d1 <= d2 (within a DELTA)
*/
@@ -147,36 +127,90 @@ aspect Navigation {
}
aspect Printing {
- syn String LogicalScene.prettyPrint() {
+ //--- prettyPrint ---
+ syn String JastAddList.prettyPrint() {
+ return prettyPrint(Object::toString);
+ }
+ syn String JastAddList.prettyPrint(java.util.function.Function<T, String> toString) {
+ return java.util.stream.StreamSupport.stream(Spliterators.spliteratorUnknownSize(this.iterator(), 16), false).map(toString::apply).collect(java.util.stream.Collectors.joining(", ", "[", "]"));
+ }
+
+ syn String Scene.prettyPrint() {
StringBuilder sb = new StringBuilder();
- for (var location : getLogicalDropOffLocationList()) {
- sb.append("location ").append(location);
- if (!location.getContainedObjectList().isEmpty()) {
- sb.append(" (objects: ").append(location.getContainedObjectList()).append(")");
+ if (getNumDropOffLocation() == 0) {
+ sb.append(" no locations\n");
+ } else {
+ for (var location : getDropOffLocationList()) {
+ sb.append(" location ").append(location.prettyPrint()).append("\n");
}
- sb.append("\n");
}
- for (var obj : getLogicalMovableObjectList()) {
- sb.append("obj ").append(obj);
- if (obj.hasLocatedAt()) {
- sb.append(" (locatedAt: ").append(obj.getLocatedAt()).append(")");
+ if (getNumMovableObject() == 0) {
+ sb.append(" no objects\n");
+ } else {
+ for (var obj : getMovableObjectList()) {
+ sb.append(" obj ").append(obj.prettyPrint()).append("\n");
}
- sb.append("\n");
}
return sb.toString();
}
- syn String JastAddList.prettyPrint() {
- return prettyPrint(Object::toString);
+
+ syn String MovableObject.prettyPrint() {
+ return "<obj " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "@" + Integer.toHexString(hashCode()) + ">";
}
- syn String JastAddList.prettyPrint(java.util.function.Function<T, String> toString) {
- return java.util.stream.StreamSupport.stream(Spliterators.spliteratorUnknownSize(this.iterator(), 16), false).map(toString::apply).collect(java.util.stream.Collectors.joining(", ", "[", "]"));
+
+ syn String DropOffLocation.prettyPrint() {
+ return "{loc " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "@" + Integer.toHexString(hashCode()) + "}";
+ }
+
+ syn String Position.prettyPrint() {
+ return " pos=(" + getX() + "," + getY() + "," + getZ() + ")";
+ }
+
+ syn String Orientation.prettyPrint() {
+ return " orient=(" + getX() + "," + getY() + "," + getZ() + "," + getW() + ")";
+ }
+
+ syn String Size.prettyPrint() {
+ return " size=(" + getLength() + "," + getWidth() + "," + getHeight() + ")";
+ }
+
+ syn String LogicalScene.prettyPrint() {
+ StringBuilder sb = new StringBuilder();
+ if (getNumLogicalDropOffLocation() == 0) {
+ sb.append(" no locations\n");
+ } else {
+ for (var location : getLogicalDropOffLocationList()) {
+ sb.append(" location ").append(location.prettyPrint());
+ if (!location.getContainedObjectList().isEmpty()) {
+ sb.append(" (objects: ").append(location.getContainedObjectList()).append(")");
+ }
+ sb.append("\n");
+ }
+ }
+ if (getNumLogicalMovableObject() == 0) {
+ sb.append(" no objects\n");
+ } else {
+ for (var obj : getLogicalMovableObjectList()) {
+ sb.append(" obj ").append(obj.prettyPrint());
+ if (obj.hasLocatedAt()) {
+ sb.append(" (locatedAt: ").append(obj.getLocatedAt()).append(")");
+ }
+ sb.append("\n");
+ }
+ }
+ return sb.toString();
}
- syn String LogicalMovableObject.toString() = "<obj " + getName() + "@" + hashCode() + ">";
- syn String LogicalDropOffLocation.toString() = "{loc " + getName() + "@" + hashCode() + "}";
+ syn String LogicalMovableObject.prettyPrint() {
+ return "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">";
+ }
+ syn String LogicalDropOffLocation.prettyPrint() {
+ return "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}";
+ }
}
aspect ConvenienceMethods {
+ // --- of ---
public static DropOffLocation DropOffLocation.of(String name,
Position position, Orientation orientation, Size size) {
var location = new DropOffLocation();
@@ -211,3 +245,7 @@ aspect ConvenienceMethods {
return new Size(length, width, height);
}
}
+
+aspect Glue {
+ class MqttHandler implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.MqttHandlerWrapper<MqttHandler> {}
+}
diff --git a/ros3rag.common/src/main/resources/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle
index 7b7c51f49605ed3cfb0eaa059031c06ca15fb4f4..8016213bdb698da3a2458d150e84f6574ec4f03e 100644
--- a/ros3rag.common/src/main/resources/tasks.gradle
+++ b/ros3rag.common/src/main/resources/tasks.gradle
@@ -1,10 +1,8 @@
-apply plugin: 'jastadd'
-
dependencies {
- jastadd2 "org.jastadd:jastadd:2.3.5"
+ jastadd2 "org.jastadd:jastadd2:2.3.5-dresden-5"
+// jastadd2 fileTree(include: ['jastadd2.jar'], dir: '../libs')
api group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
-
}
sourceCompatibility = 11
@@ -15,7 +13,7 @@ targetCompatibility = 11
task ragConnect(type: JavaExec) {
group = 'Build'
main = 'org.jastadd.ragconnect.compiler.Compiler'
- classpath = configurations.ragconnectClasspath
+ classpath = configurations.ragconnect
args([
'--o=src/gen/jastadd',
@@ -27,7 +25,9 @@ task ragConnect(type: JavaExec) {
'--logWrites',
// '--verbose',
'--rootNode=' + project.ext.ragConnectRootNode,
- '--incremental=param,debug',
+ '--List=JastAddList',
+ '--experimental-jastadd-329',
+ '--incremental=param',
"--tracing=cache,flush"
])
@@ -38,32 +38,35 @@ task ragConnect(type: JavaExec) {
task grammar2uml(type: JavaExec) {
main = 'de.tudresden.inf.st.jastadd.grammar2uml.compiler.Compiler'
- classpath = configurations.grammar2umlClasspath
+ classpath = configurations.grammar2uml
args([
'--verbose',
'src/gen/jastadd/types.relast'
- ]
- +
- project.ext.relastFiles)
+ ] + project.ext.relastFiles)
}
// phase: RelAst
task relastToJastAdd(type: JavaExec) {
group = 'Build'
- main = "-jar"
+ classpath = configurations.relast
+ //noinspection GroovyAssignabilityCheck, GroovyAccessibility
+ mainClass = 'org.jastadd.relast.compiler.Compiler'
+
+ doFirst {
+ mkdir "src/gen/jastadd/"
+ }
- args(["../libs/relast.jar",
- "--grammarName=./src/gen/jastadd/model",
+ args = [
+ "--grammarName=./src/gen/jastadd/model",
"--useJastAddNames",
"--listClass=java.util.ArrayList",
"--jastAddList=JastAddList",
"--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")
@@ -119,7 +122,7 @@ jastadd {
'--lineColumnNumbers',
'--List=JastAddList',
'--cache=all',
- "--flush=full",
+ "--flush=api",
"--incremental=param,debug",
"--tracing=cache,flush",
]
diff --git a/ros3rag.placeA/build.gradle b/ros3rag.placeA/build.gradle
index 69f31e94c823d394dc6929e1b7145ba5c94bbe59..5e90f00c02c0ffe44d5e15740f9d6c6702702c4e 100644
--- a/ros3rag.placeA/build.gradle
+++ b/ros3rag.placeA/build.gradle
@@ -1,7 +1,7 @@
buildscript {
repositories.mavenCentral()
dependencies {
- classpath group: 'org.jastadd', name: 'jastaddgradle', version: '1.13.3'
+ classpath group: 'org.jastadd', name: 'jastaddgradle', version: "${jastadd_gradle_version}"
}
}
diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.connect b/ros3rag.placeA/src/main/jastadd/WorldModelA.connect
index aa4b1c84fbd0bc8f63fc5e259d73d9e6760e968f..c3e16e8da32f55d35ca7eb6ce2bcfdf7e8f2b57f 100644
--- a/ros3rag.placeA/src/main/jastadd/WorldModelA.connect
+++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.connect
@@ -1,13 +1,13 @@
// --- receiving ---
-receive tree WorldModelA.Scene using ParseScene, ConvertScene ;
+receive WorldModelA.Scene using ParseScene, ConvertScene ;
// rs: not sure whether we want to receive a complete scene. could be hard to update/merge
// if necessary, need a mapping. the multi-point-connection is an alternative.
// we need to decide for one way
// rs: let's try to use a "multi-point-connection" (same input meesage, multiple recipients)
-receive tree ObjectOfInterest.Position using DeserializeObject, ExtractPositionFromObject ;
-receive tree ObjectOfInterest.Size using DeserializeObject, ExtractSizeFromObject ;
-receive tree ObjectOfInterest.Orientation using DeserializeObject,ExtractOrientationFromObject ;
+receive ObjectOfInterest.Position using DeserializeObject, ExtractPositionFromObject ;
+receive ObjectOfInterest.Size using DeserializeObject, ExtractSizeFromObject ;
+receive ObjectOfInterest.Orientation using DeserializeObject,ExtractOrientationFromObject ;
DeserializeObject maps byte[] bytes to de.tudresden.inf.st.ceti.Object {:
return de.tudresden.inf.st.ceti.Object.parseFrom(bytes);
@@ -38,4 +38,4 @@ ExtractOrientationFromObject maps de.tudresden.inf.st.ceti.Object o to Orientati
:}
// --- sending ---
-send tree Scene.LogicalScene ;
+send WorldModelA.LogicalScene ;
diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd b/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd
index 3ad5caed414eab31b1b1c2223ed552184c54a022..b9dd1eebab6bf09ba350bce028e8f6bac4349695 100644
--- a/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd
+++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd
@@ -1,33 +1,14 @@
-import java.util.List;
-import java.util.ArrayList;
-import java.util.Map;
-import java.util.HashMap;
-
aspect Resolving {
+ //--- resolveObjectOfInterest ---
inh ObjectOfInterest Robot.resolveObjectOfInterest(String name);
eq WorldModelA.getRobot().resolveObjectOfInterest(String name) {
return getScene().resolveObjectOfInterest(name);
}
}
-aspect Computation {
-// syn LogicalScene WorldModelA.getLogicalScene() {
-// LogicalScene result = new LogicalScene();
-// Map<MovableObject, LogicalMovableObject> objects = new HashMap<>();
-// for (DropOffLocation location : getScene().getDropOffLocationList()) {
-// var logicalLocation = new LogicalDropOffLocation();
-// logicalLocation.setName(location.getName());
-// for (MovableObject movableObject : getScene().getMovableObjectList()) {
-// if (movableObject.isLocatedAt(location)) {
-// LogicalMovableObject logicalMovableObject = objects.computeIfAbsent(movableObject, k -> {
-// var newLogicalMovableObject = new LogicalMovableObject();
-// newLogicalMovableObject.setName(k.getName());
-// return newLogicalMovableObject;
-// });
-// logicalLocation.addContainedObjects(logicalMovableObject);
-// }
-// }
-// }
-// return result;
-// }
+aspect Glue {
+ class WorldModelA implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {}
+
+ //--- getLogicalScene ---
+ syn LogicalScene WorldModelA.getLogicalScene() = hasScene() ? getScene().getLogicalScene() : new LogicalScene();
}
diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
index 67c9b50130f87fa614a10a7381fb0ca6f72404dc..4089553a078bf8fb2d1cbdda036562f9ce309b68 100644
--- a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
+++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast
@@ -1,5 +1,3 @@
-WorldModelA ::= Robot [Scene] ; //Task* ;
+WorldModelA ::= Robot [Scene] /LogicalScene/ ;
Robot ::= <Name:String> ;
-
-//Task ::= ObjectToMove:MovableObject TargetLocation:DropOffLocation ;
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 83ca62dd2629f663b2684111fd594b4c91d4e689..157de87d3bbaed53de0fb47e66df021cc2ef3d02 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
@@ -4,14 +4,10 @@ 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.SharedMainParts;
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.util.concurrent.CountDownLatch;
-import java.util.concurrent.TimeUnit;
+import java.io.IOException;
import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
@@ -21,41 +17,47 @@ import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
*
* @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;
-
- public static void main(String[] args) throws Exception {
- new MainA().run(args);
+public class MainA extends SharedMainParts<MqttHandler, WorldModelA> {
+ private final String TOPIC_SCENE_UPDATE_FROM_ROS;
+ private final String TOPIC_SCENE_UPDATE_TO_PLACE_B;
+
+ private final String TOPIC_DEMO_MOVE_objectRed1_BLUE;
+ private final String TOPIC_DEMO_MOVE_objectRed1_RED;
+ private final String TOPIC_EVAL_MOVE;
+
+ MainA(String configFile) {
+ super("place-a", UtilA.pathToDirectoryOfPlaceA().resolve(configFile));
+ this.TOPIC_SCENE_UPDATE_FROM_ROS = cellName + "/scene/update";
+ this.TOPIC_SCENE_UPDATE_TO_PLACE_B = cellName + "/logical/update";
+
+ this.TOPIC_DEMO_MOVE_objectRed1_BLUE = cellName + "/demo/move/objectRed1/blue";
+ this.TOPIC_DEMO_MOVE_objectRed1_RED = cellName + "/demo/move/objectRed1/red";
+ this.TOPIC_EVAL_MOVE = cellName + "/eval/move";
}
- private void run(String[] args) throws Exception {
+ public static void main(String[] args) throws Exception {
String configFile = args.length == 0 ? "src/main/resources/config-a.yaml" : args[0];
+ new MainA(configFile).run();
+ }
- // --- No configuration below this line ---
-
- final var config = Util.parseConfig(UtilA.pathToDirectoryOfPlaceA().resolve(configFile).toFile());
-
- model = new WorldModelA();
-
- /// 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)));
+ @Override
+ protected void createSpecificMainHandlerConnections() {
+ mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_BLUE, bytes ->
+ UtilA.updatePositionOfObjectToLocation(model.getScene(), "objectRed1", "binBlue")
+ );
+ mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
+ UtilA.updatePositionOfObjectToLocation(model.getScene(), "objectRed1", "binRed")
+ );
+ mainHandler.newConnection(TOPIC_EVAL_MOVE, bytes -> {
+ String[] tokens = new String(bytes).split(" to ");
+ String objectName = tokens[0];
+ String locationName = tokens[1];
+ UtilA.updatePositionOfObjectToLocation(model.getScene(), objectName, locationName);
+ });
+ }
+ @Override
+ protected de.tudresden.inf.st.ceti.Scene readSceneAndRobots() throws Exception {
/// Reading scene and robot
de.tudresden.inf.st.ceti.Scene scene = readScene(
UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene)
@@ -64,48 +66,34 @@ public class MainA {
model.setScene(myScene);
// 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);
+ Util.extractRobotNames(scene).forEach(name -> {
+ if (model.getRobot() != null) {
+ logger.error("Robot already set. Overriding with newly found name.");
+ }
+ model.setRobot(new Robot().setName(name));
+ });
+
+ return scene;
+ }
- /// Connect endpoints
+ @Override
+ protected void connectEndpoints() throws IOException {
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);
-
-// 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();
+ model.connectLogicalScene(mqttUri(TOPIC_SCENE_UPDATE_TO_PLACE_B, config), true);
}
- 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));
+ @Override
+ protected MqttHandler createMqttHandler() {
+ return new MqttHandler("mainHandlerA");
}
- private void logStatus(String prefix) {
- logger.info(prefix);
- String content = UtilA.getModelInfos(model);
- logger.info("WorldModelA\n{}", content);
- if (mainHandler != null) {
- mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8));
- }
+ @Override
+ protected WorldModelA createWorldModel() {
+ return new WorldModelA();
}
- private void close() {
- logger.info("Exiting ...");
- mainHandler.close();
- model.ragconnectCloseConnections();
+ @Override
+ protected String getModelInfos(WorldModelA worldModelA, boolean detailed) {
+ return UtilA.getModelInfos(model, detailed);
}
}
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 327b5b5ec5cab962a7f36108fc4125cb3dc0ac0f..de2074c5219b619765ce87ca40446bcac35f267a 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
@@ -59,7 +59,7 @@ public class SimpleMainA {
model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
model.connectScene("mqtt://localhost/scene/update");
- model.getScene().connectLogicalScene("mqtt://localhost/logical/scene/update", true);
+ model.connectLogicalScene("mqtt://localhost/logical/scene/update", true);
}
@SuppressWarnings("unused")
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
index c7910770b58214cc10b25f6b393c2f45e6e3ea6a..fafa021fed70482ede4922a531724e386b8e1adc 100644
--- 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
@@ -1,8 +1,6 @@
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.placeA.ast.*;
import de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
@@ -25,11 +23,39 @@ public class UtilA {
return new ExposingASTNode().exposed_apply_ConvertScene(scene);
}
- public static String getModelInfos(WorldModelA model) {
+ static void updatePositionOfObjectToLocation(Scene scene, String objName, String locationName) {
+ ObjectOfInterest obj = scene.resolveObjectOfInterest(objName);
+ ObjectOfInterest location = scene.resolveObjectOfInterest(locationName);
+ if (obj != null && location != null) {
+ // move objectRed1 to binBlue
+ logger.info("Got " + obj + " and location " + location);
+ logger.debug("before to {} at {}\n{}", locationName, location.getPosition(),
+ scene.prettyPrint());
+
+ obj.setPosition(Position.of(location.getPosition().getX(),
+ location.getPosition().getY(),
+ location.getPosition().getZ()));
+
+ logger.debug("after\n{}", scene.prettyPrint());
+ } else {
+ logger.error("Obj (" + obj + ") or location (" + location + ") are null");
+ }
+ }
+
+ public static String getModelInfos(WorldModelA model, boolean detailed) {
StringBuilder sb = new StringBuilder();
sb.append("myRobot: ")
.append(model.getRobot().getName())
.append("\n");
+ if (detailed) {
+ // also include "normal" scene
+ sb.append("myScene:");
+ if (model.hasScene()) {
+ sb.append("\n").append(model.getScene().prettyPrint());
+ } else {
+ sb.append(" (unset)\n");
+ }
+ }
sb.append("myLogicalScene:");
if (model.hasScene()) {
sb.append("\n").append(model.getScene().getLogicalScene().prettyPrint());
@@ -42,7 +68,7 @@ public class UtilA {
@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);
+ return ASTNode._ragconnect__apply_ConvertScene(pbScene);
}
}
}
diff --git a/ros3rag.placeA/src/main/resources/config-a.yaml b/ros3rag.placeA/src/main/resources/config-a.yaml
index a3901c56e167bfdf68e837111068528b5ca991f2..ab5d9af69a148c11b82c1f3301c8b0e2727bad18 100644
--- a/ros3rag.placeA/src/main/resources/config-a.yaml
+++ b/ros3rag.placeA/src/main/resources/config-a.yaml
@@ -1,3 +1,4 @@
mqttHost: "localhost"
filenameInitialScene: "src/main/resources/config-scene-a.json"
useReachability: false
+coordinatorMqttTopicPrefix: "coordinating/rag-a"
diff --git a/ros3rag.placeA/src/main/resources/config-scene-a.json b/ros3rag.placeA/src/main/resources/config-scene-a.json
index 9f09d081b7d6caa749305865649366b180d27286..92b9ce4a0dde73a3379c4bb3cab0f3abfe892978 100644
--- a/ros3rag.placeA/src/main/resources/config-scene-a.json
+++ b/ros3rag.placeA/src/main/resources/config-scene-a.json
@@ -4,17 +4,21 @@
{ "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": "binBlue","type": "DROP_OFF_LOCATION","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": "DROP_OFF_LOCATION","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": "DROP_OFF_LOCATION","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": "binYellow","type": "DROP_OFF_LOCATION","pos": { "x": 1.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } },
+ { "id": "binPurple","type": "DROP_OFF_LOCATION","pos": { "x": 2.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 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": "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": 0.382683,"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": "objectYellow1","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, "g": 1 } },
+ { "id": "objectPurple1","type": "BOX","pos": { "x": 0.3,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1, "b": 1 } },
{ "id": "arm","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }
] }
diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle
index 95d2d16ba73432095be516bff69b187c4fcd047f..0df9901804825aed51c43f9ed284a383c4b03e38 100644
--- a/ros3rag.placeB/build.gradle
+++ b/ros3rag.placeB/build.gradle
@@ -2,7 +2,7 @@ buildscript {
repositories.mavenLocal()
repositories.mavenCentral()
dependencies {
- classpath group: 'org.jastadd', name: 'jastaddgradle', version: '1.13.3'
+ classpath group: 'org.jastadd', name: 'jastaddgradle', version: "${jastadd_gradle_version}"
}
}
@@ -17,18 +17,18 @@ task simpleRun(type: JavaExec) {
group 'application'
classpath sourceSets.main.runtimeClasspath
main = "de.tudresden.inf.st.placeB.SimpleMainB"
-
}
dependencies {
implementation project(':ros3rag.common')
+ testImplementation group: 'org.junit.jupiter', name: 'junit-jupiter-params', version: "${jupiter_version}"
}
ext.sharedJastAddDir = 'src/main/jastadd/shared'
ext.ragConnectInputGrammar = 'src/main/jastadd/WorldModelB.relast'
ext.ragConnectInputConnect = 'src/main/jastadd/WorldModelB.connect'
ext.ragConnectRootNode = 'WorldModelB'
-ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/gen/jastadd/RagConnect.relast"]
+ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/main/jastadd/BFS/BFS.relast", "src/main/jastadd/RobotReachabilityToBFS.relast", "src/gen/jastadd/RagConnect.relast"]
ext.jastaddAstPackage = 'de.tudresden.inf.st.placeB.ast'
apply from: '../ros3rag.common/src/main/resources/tasks.gradle'
diff --git a/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..d59de4fa0aa754a46085e640eff698831da30728
--- /dev/null
+++ b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag
@@ -0,0 +1,67 @@
+import java.util.*;
+
+aspect BFS {
+ //--- BFS ---
+ syn List<Edge> Vertex.BFS(Vertex goal) {
+ Map<Vertex, Edge> pred = new HashMap<>();
+ Queue<Vertex> Q = new LinkedList<>();
+ Set<Vertex> seen = new HashSet<>();
+ seen.add(this);
+ Q.add(this);
+ while (!Q.isEmpty()) {
+ Vertex v = Q.remove();
+ if (v.equals(goal)) {
+ return reconstructFrom(goal, pred);
+ }
+ for (Edge e : v.getOutgoingList()) {
+ Vertex w = e.getTo();
+ if (seen.contains(w)) {
+ continue;
+ }
+ seen.add(w);
+ pred.put(w, e);
+ Q.add(w);
+ }
+ for (Edge e : v.getIncomingList()) {
+ if (!e.getBidirectional()) { continue; }
+ Vertex w = e.getFrom();
+ if (seen.contains(w)) {
+ continue;
+ }
+ seen.add(w);
+ pred.put(w, e);
+ Q.add(w);
+ }
+ }
+ // failure
+ return null;
+ }
+
+ //--- reconstructFrom ---
+ private List<Edge> Vertex.reconstructFrom(Vertex goal, Map<Vertex, Edge> pred) {
+ // used at the end of BFS to back-track path from goal to start
+ List<Edge> result = new ArrayList<>();
+ Vertex current = goal;
+ while (!current.equals(this)) {
+ Edge e = pred.get(current);
+ result.add(e);
+ current = e.getFrom().equals(current) ? e.getTo() : e.getFrom();
+ }
+// result.add(this);
+ Collections.reverse(result);
+ return result;
+ }
+
+ //--- prettyPrint ---
+ public String Graph.prettyPrint() {
+ StringJoiner sj = new StringJoiner("\n", "Graph {\n", "\n}");
+ if (!hasVertex()) { sj.add("<no vertices>"); }
+ for (Vertex v : getVertexList()) { sj.add(v.toString()); }
+ if (!hasEdge()) { sj.add("<no edges>"); }
+ for (Edge e : getEdgeList()) { sj.add(e.prettyPrint()); }
+ return sj.toString();
+ }
+ public String Edge.prettyPrint() {
+ return this.toString() + " : " + getFrom() + (getBidirectional() ? " <-> " : " -> ") + getTo();
+ }
+}
diff --git a/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast b/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast
new file mode 100644
index 0000000000000000000000000000000000000000..c3931ad536b40be9d04dfe733a542e8f56360c37
--- /dev/null
+++ b/ros3rag.placeB/src/main/jastadd/BFS/BFS.relast
@@ -0,0 +1,5 @@
+Graph ::= Vertex* Edge* ;
+Vertex ;
+Edge ::= <Bidirectional:boolean> ;
+rel Edge.From <-> Vertex.Outgoing* ;
+rel Edge.To <-> Vertex.Incoming* ;
diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..cced6d479cb6ec7f922f67a059c895923ea9d2e3
--- /dev/null
+++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag
@@ -0,0 +1,62 @@
+aspect RobotReachabilityToBFS {
+ //--- toReachabilityGraph ---
+ syn nta Graph WorldModelB.toReachabilityGraph() {
+ Graph result = new Graph();
+ if (!hasMyScene()) {
+ return result;
+ }
+ Map<LogicalObjectOfInterest, Vertex> mapping = new HashMap<>();
+ for (LogicalDropOffLocation loc : getMyScene().getLogicalScene().getLogicalDropOffLocationList()) {
+ Vertex vertex = new Vertex();
+ vertex.setObjectOfInterest(loc);
+ result.addVertex(vertex);
+ mapping.put(loc, vertex);
+ }
+ for (LogicalMovableObject obj : getMyScene().getLogicalScene().getLogicalMovableObjectList()) {
+ Vertex vertex = new Vertex();
+ vertex.setObjectOfInterest(obj);
+ result.addVertex(vertex);
+ mapping.put(obj, vertex);
+ }
+ for (Robot robot : getRobotList()) {
+ List<LogicalObjectOfInterest> reachableLocations = robot.reachableObjects().stream()
+ .filter(LogicalObjectOfInterest::isLogicalDropOffLocation)
+ .collect(java.util.stream.Collectors.toList());
+ // bidirectional edges between locations, unidirectional edges from all reachable movableobjects to locations
+ for (LogicalObjectOfInterest obj : robot.reachableObjects()) {
+ for (LogicalObjectOfInterest other : reachableLocations) {
+ if (obj == other) { continue; }
+ Edge edge = new Edge();
+ edge.setRobot(robot);
+ edge.setBidirectional(obj.isLogicalDropOffLocation());
+ edge.setFrom(mapping.get(obj));
+ edge.setTo(mapping.get(other));
+ result.addEdge(edge);
+ }
+ }
+ }
+ return result;
+ }
+
+ //--- correspondingVertex ---
+ syn Optional<Vertex> LogicalObjectOfInterest.correspondingVertex() {
+ if (!worldModelB().hasMyScene()) {
+ return Optional.empty();
+ }
+ for (Vertex v : worldModelB().toReachabilityGraph().getVertexList()) {
+ if (v.getObjectOfInterest().getName().equals(this.getName())) {
+ return Optional.of(v);
+ }
+ }
+ return Optional.empty();
+ }
+}
+
+aspect Printint {
+ public String Vertex.toString() {
+ return "V(" + getObjectOfInterest().getName() + ")";
+ }
+ public String Edge.toString() {
+ return "E[" + getRobot().getName() + "]";
+ }
+}
diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast
new file mode 100644
index 0000000000000000000000000000000000000000..62d0b4071475bea2960b6aed0becbad54aa9eaaf
--- /dev/null
+++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast
@@ -0,0 +1,2 @@
+rel Vertex.ObjectOfInterest -> LogicalObjectOfInterest ;
+rel Edge.Robot -> Robot ;
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index f77374792f0d58550ceb3515913b5a5a6f802752..d2a9fab1ffc9a1e03bed700abce9002545137088 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -1,11 +1,17 @@
// --- receiving ---
-receive tree WorldModelB.MyScene using ParseScene, ConvertScene ;
-receive tree WorldModelB.OtherScene ;
-receive tree Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
+receive WorldModelB.MyScene using ParseScene, ConvertScene ;
+receive WorldModelB.OtherScene1 ;
+receive WorldModelB.OtherScene2 ;
+receive WorldModelB.TestingOtherScene ;
+receive Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ;
// --- sending ---
-send tree WorldModelB.NextOperation using PrintOperation ;
+send WorldModelB.NextOperation using PrintOperation ;
PrintOperation maps Operation op to byte[] {:
- return op.toMergedSelection().toByteArray();
+ var result = op.toMergedSelection();
+ if (result == null) {
+ reject();
+ }
+ return result.toByteArray();
:}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index 047ade07f567ab5e163fb7f04bace5daad17057b..119cccc15f630c7968c7370f628de6db60e0a847 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -1,36 +1,120 @@
aspect Computation {
+ //--- unspecifiedLocation ---
+ syn nta LogicalDropOffLocation WorldModelB.unspecifiedLocation() = new LogicalDropOffLocation().setName("<unspecified>");
+
+ //--- getOtherSceneList ---
+ syn JastAddList<LogicalScene> WorldModelB.getOtherSceneList() {
+ JastAddList<LogicalScene> result = new JastAddList<>();
+ if (hasOtherScene1()) {
+ result.add(getOtherScene1());
+ }
+ if (hasOtherScene2()) {
+ result.add(getOtherScene2());
+ }
+ return result;
+ }
+
+ //--- mergedOtherScene ---
+ syn nta LogicalScene WorldModelB.mergedOtherScene() {
+ // return empty scene, if there are no other scenes (yet)
+ if (!hasOtherScene()) {
+ return new LogicalScene();
+ }
+ // return scene, if there is exactly one. (TODO: check if copy is needed here)
+ if (getNumOtherScene() == 1) {
+ return getOtherScene(0);
+ }
+ var result = new LogicalScene();
+ // use getOtherSceneList() and merge by name all locations, objects and relations into one new, unified scene
+ Map<String, LogicalDropOffLocation> mergedLocations = new HashMap<>();
+ Map<String, LogicalMovableObject> mergedObjects = new HashMap<>();
+ // first create all locations
+ for (LogicalScene scene : getOtherSceneList()) {
+ for (LogicalDropOffLocation loc : scene.getLogicalDropOffLocationList()) {
+ if (!mergedLocations.containsKey(loc.getName())) {
+ LogicalDropOffLocation copyOfLoc = new LogicalDropOffLocation();
+ copyOfLoc.setName(loc.getName());
+ mergedLocations.put(loc.getName(), copyOfLoc);
+ result.addLogicalDropOffLocation(copyOfLoc);
+ }
+ }
+ }
+ // then create all objects, and set LocatedAt relation (checking for possible conflicts)
+ for (LogicalScene scene : getOtherSceneList()) {
+ for (LogicalMovableObject obj : scene.getLogicalMovableObjectList()) {
+ if (mergedObjects.containsKey(obj.getName())) {
+ // object already known
+ LogicalMovableObject copyOfObj = mergedObjects.get(obj.getName());
+ if (obj.hasLocatedAt()) {
+ String locationName = obj.getLocatedAt().getName();
+ LogicalDropOffLocation copyOfLoc = mergedLocations.get(locationName);
+ if (copyOfObj.hasLocatedAt() && !obj.getLocatedAt().getName().equals(copyOfObj.getLocatedAt().getName())) {
+ System.err.println("Overriding located-at of " + obj.getName() + " while merging (" + locationName + "," + copyOfObj.getLocatedAt().getName() + ")");
+ }
+ copyOfObj.setLocatedAt(copyOfLoc);
+ }
+ } else {
+ LogicalMovableObject copyOfObj = new LogicalMovableObject();
+ copyOfObj.setName(obj.getName());
+ if (obj.hasLocatedAt()) {
+ copyOfObj.setLocatedAt(mergedLocations.get(obj.getLocatedAt().getName()));
+ }
+ mergedObjects.put(obj.getName(), copyOfObj);
+ result.addLogicalMovableObject(copyOfObj);
+ }
+ }
+ }
+ return result;
+ }
+
+ //--- diffScenes ---
syn nta JastAddList<Difference> WorldModelB.diffScenes() {
var result = new JastAddList<Difference>();
if (!hasMyScene() || !hasOtherScene()) {
- // without both scenes, there are no differences
+ // without my and at least one other scene, there are no differences
return result;
}
var myLogicalScene = getMyScene().getLogicalScene();
- for (LogicalMovableObject otherObject : getOtherScene().getLogicalMovableObjectList()) {
+ for (LogicalMovableObject otherObject : mergedOtherScene().getLogicalMovableObjectList()) {
LogicalObjectOfInterest myGenericObject = myLogicalScene.resolveLogicalObjectOfInterest(otherObject.getName());
+ if (myGenericObject == null) {
+ // new object, currently not handled
+ var diff = new DifferenceNewObject();
+ diff.setObject(otherObject);
+ result.add(diff);
+ continue;
+ }
if (!myGenericObject.isLogicalMovableObject()) {
System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!");
continue;
}
LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject();
- if (myObject == null) {
- // new object, currently not handled
- result.add(new DifferenceNewObject());
+ if (myObject.hasLocatedAt() && !otherObject.hasLocatedAt()) {
+ var diff = new DifferenceObjectMisplaced();
+ diff.setObject(myObject);
+ diff.setPreviousLocation(myObject.getLocatedAt());
+ result.add(diff);
+ continue;
+ }
+ 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;
}
- if (!myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName())) {
- // object at different locations
- var difference = new DifferenceObjectAtWrongPlace();
- difference.setObject(myObject);
+ 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
+ difference.setNewLocation(otherObject.getLocatedAt());
+ result.add(difference);
}
return result;
}
+ //--- diffToOperations ---
syn nta JastAddList<Operation> WorldModelB.diffToOperations() {
var result = new JastAddList<Operation>();
for (Difference difference : diffScenes()) {
@@ -41,59 +125,41 @@ aspect Computation {
return result;
}
+ //--- computeOperations ---
syn List<Operation> Difference.computeOperations();
eq DifferenceObjectAtWrongPlace.computeOperations() {
- // first, find robots that can reach the locations
- // there does not necessarily has to be one robot that can reach both
- Set<Robot> robotsFitForPreviousLocation = new HashSet<>();
- Set<Robot> robotsFitForNewLocation = new HashSet<>();
- for (var robot : worldModelB().getRobotList()) {
- if (robot.canReach(getPreviousLocation().getName())) {
- robotsFitForPreviousLocation.add(robot);
- }
- if (robot.canReach(getNewLocation().getName())) {
- robotsFitForNewLocation.add(robot);
- }
- }
- if (robotsFitForPreviousLocation.isEmpty()) {
- return Collections.singletonList(new ErrorOperation("No robot can reach previous location " + getPreviousLocation().getName()));
- }
- if (robotsFitForNewLocation.isEmpty()) {
- return Collections.singletonList(new ErrorOperation("No robot can reach new location " + getNewLocation().getName()));
- }
- var result = new ArrayList<Operation>();
- var robotFitForBothLocations = robotsFitForPreviousLocation.stream()
- .filter(robotsFitForNewLocation::contains)
- .findFirst();
- robotFitForBothLocations.ifPresentOrElse(robot -> {
- // there is at least one robot that can reach both locations
- result.add(createPickAndPlace(robot, this.getObject(), this.getNewLocation()));
- },
- () -> {
- // no single robot can reach both locations
- // 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.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
- String objectName = canReachOfRobotForPrevious.getObjectName();
- if (resolveLogicalObjectOfInterest(objectName).isLogicalDropOffLocation() && robotForNew.canReach(objectName)) {
- // add two operations.
- // obj -> transitLoc by robotForPrevious
- // obj -> targetLoc by robotForNew
- result.add(createPickAndPlace(robotForPrevious, this.getObject(), resolveLogicalObjectOfInterest(objectName).asLogicalDropOffLocation()));
- result.add(createPickAndPlace(robotForNew, this.getObject(), this.getNewLocation()));
- }
+ return (hasPreviousLocation() ? getPreviousLocation() : getObject()).correspondingVertex().map(previousVertex -> {
+ return getNewLocation().correspondingVertex().map(newVertex -> {
+ List<Edge> shortestPath = previousVertex.BFS(newVertex);
+ if (shortestPath == null || shortestPath.isEmpty()) {
+ return error("No sequence of operations to move " + getObject().getName() + (hasPreviousLocation() ? " from " + getPreviousLocation().getName() : "") + " to " + getNewLocation().getName());
}
- }
- }
- });
- return result;
+ List<Operation> result = new ArrayList<>();
+ Vertex transit = previousVertex;
+ for (Edge edge : shortestPath) {
+ Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
+ result.add(createPickAndPlace(edge.getRobot(), getObject(), target.getObjectOfInterest().asLogicalDropOffLocation()));
+ transit = target;
+ }
+ return result;
+ }).orElseGet(() -> error("Could not resolve graph vertex of new location " + getNewLocation().getName()));
+ }).orElseGet(() -> error("Could not resolve graph vertex of previous location " + getPreviousLocation().getName()));
}
eq DifferenceNewObject.computeOperations() {
// FIXME. stub, may be implemented later
return Collections.emptyList();
}
+ eq DifferenceObjectMisplaced.computeOperations() {
+ // FIXME. stub, may be implemented later
+ return Collections.emptyList();
+ }
+ //--- error ---
+ protected static List<Operation> Difference.error(String message) {
+ return Collections.singletonList(new ErrorOperation(message));
+ }
+
+ //--- createPickAndPlace ---
private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) {
var result = new PickAndPlace();
result.setRobotToExecute(robot);
@@ -102,12 +168,20 @@ aspect Computation {
return result;
}
+ //--- getNextOperation ---
syn Operation WorldModelB.getNextOperation() {
- return diffToOperations().getNumChild() > 0 ? diffToOperations().getChild(0) : new ErrorOperation("No operation computed!");
+ if (diffToOperations().getNumChild() == 0) {
+ return new ErrorOperation("No operation computed!");
+ }
+ for (Operation op : diffToOperations()) {
+ if (!op.isErrorOperation()) {
+ return op;
+ }
+ }
+ return new ErrorOperation("No executable operation found!");
}
-// syn List<String> locationIds
-
+ //--- canReach ---
syn boolean Robot.canReach(String objectName) {
for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
if (canReachObj.getObjectName().equals(objectName)) {
@@ -116,6 +190,18 @@ aspect Computation {
}
return false;
}
+
+ //--- reachableObjects ---
+ syn List<LogicalObjectOfInterest> Robot.reachableObjects() {
+ if (!worldModelB().hasMyScene()) {
+ return Collections.emptyList();
+ }
+ List<LogicalObjectOfInterest> result = new ArrayList<>();
+ for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
+ result.add(worldModelB().getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(canReachObj.getObjectName()));
+ }
+ return result;
+ }
}
//aspect RelationsByReference {
@@ -129,9 +215,11 @@ aspect Computation {
//}
aspect AttributeMappings {
+ //--- toMergedSelection ---
syn de.tudresden.inf.st.ceti.MergedSelection Operation.toMergedSelection();
eq ErrorOperation.toMergedSelection() {
- throw new RuntimeException(getErrorMessage());
+ System.err.println(getErrorMessage());
+ return null;
}
eq Pick.toMergedSelection() {
throw new RuntimeException("Separate Pick operation not supported yet!");
@@ -140,37 +228,57 @@ aspect AttributeMappings {
throw new RuntimeException("Separate Place operation not supported yet!");
}
eq PickAndPlace.toMergedSelection() = de.tudresden.inf.st.ceti.MergedSelection.newBuilder()
+ .setIdRobot(getRobotToExecute().getName())
.setIdPick(getObjectToPick().getName())
.setIdPlace(getTargetLocation().getName())
.build();
}
aspect Navigation {
+ //--- worldModelB ---
inh WorldModelB ASTNode.worldModelB();
eq WorldModelB.getChild().worldModelB() = this;
+ syn boolean Operation.isErrorOperation() = false;
+ eq ErrorOperation.isErrorOperation() = true;
}
aspect GlueForShared {
+ //--- resolveLogicalObjectOfInterest ---
// uncache Difference.resolveLogicalObjectOfInterest
inh LogicalObjectOfInterest Difference.resolveLogicalObjectOfInterest(String name);
eq WorldModelB.diffScenes().resolveLogicalObjectOfInterest(String name) = getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(name);
+
+ class WorldModelB implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {}
}
aspect Printing {
- syn String Robot.toString() = "~Robot " + getName() + "~";
+ //--- prettyPrint ---
+ syn String Robot.prettyPrint() = "~Robot " + getName() + "~";
- syn String DifferenceObjectAtWrongPlace.toString() {
- return "+DifferenceObjectAtWrongPlace of " + getObject() + ": " + getPreviousLocation() + " -> " + getNewLocation() + "+";
+ syn String Difference.prettyPrint();
+ eq DifferenceObjectAtWrongPlace.prettyPrint() {
+ return "-DifferenceObjectAtWrongPlace of " + getObject().prettyPrint() + ": " + (hasPreviousLocation() ? getPreviousLocation().prettyPrint() : "_") + " -> " + getNewLocation().prettyPrint() + "-";
}
- syn String PickAndPlace.toString() {
- return "+PickAndPlace by " + getRobotToExecute() + " of " + getObjectToPick() + " -> " + getTargetLocation() + "+";
+ eq DifferenceNewObject.prettyPrint() {
+ return "-DifferenceNewObject of " + getObject().prettyPrint() + "-";
+ }
+ eq DifferenceObjectMisplaced.prettyPrint() {
+ return "-DifferenceObjectMisplaced of " + getObject().prettyPrint() + ": " + getPreviousLocation().prettyPrint() + "-";
+ }
+
+ syn String Operation.prettyPrint();
+ eq PickAndPlace.prettyPrint() {
+ return "+PickAndPlace by " + getRobotToExecute().prettyPrint() + " of " + getObjectToPick().prettyPrint() + " -> " + getTargetLocation().prettyPrint() + "+";
}
- syn String ErrorOperation.toString() {
+ eq Pick.prettyPrint() = "+Pick not implemented+";
+ eq Place.prettyPrint() = "+Place not implemented+";
+ eq ErrorOperation.prettyPrint() {
return "+Error: " + getErrorMessage() + "+";
}
}
aspect Resolving {
+ //--- findRobot ---
public Optional<Robot> WorldModelB.findRobot(String name) {
for (Robot robot : getRobotList()) {
if (robot.getName().equals(name)) {
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index 6955828c67b6111c63c6d310571b6360a18d3ba2..f2bc3600e6bc36d75cadc98b8f6100cc8cdaa220 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
@@ -1,13 +1,16 @@
-WorldModelB ::= Robot* [MyScene:Scene] [OtherScene:LogicalScene] /NextOperation:Operation/ ;
+WorldModelB ::= Robot* [MyScene:Scene] [OtherScene1:LogicalScene] [OtherScene2:LogicalScene] /OtherScene:LogicalScene*/ /NextOperation:Operation/ TestingOtherScene:LogicalScene* ;
+// workaround with NTA OtherSceneList as receiving lists is not possible yet
Robot ::= <Name:String> CanReachObjectOfInterestWrapper ;
abstract Difference ;
+rel Difference.Object -> LogicalMovableObject ;
DifferenceObjectAtWrongPlace : Difference ;
-rel DifferenceObjectAtWrongPlace.Object -> LogicalMovableObject ;
-rel DifferenceObjectAtWrongPlace.PreviousLocation -> LogicalDropOffLocation ;
+rel DifferenceObjectAtWrongPlace.PreviousLocation? -> LogicalDropOffLocation ;
rel DifferenceObjectAtWrongPlace.NewLocation -> LogicalDropOffLocation ;
DifferenceNewObject : Difference ;
+DifferenceObjectMisplaced : Difference ;
+rel DifferenceObjectMisplaced.PreviousLocation -> LogicalDropOffLocation ;
abstract Operation ;
rel Operation.RobotToExecute? -> Robot ;
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 5548372b3bf8ec3a29bbfeab62ee46e292611885..0c193b3e77021dd6d6bcff534443a1d197d269d3 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
@@ -3,16 +3,12 @@ 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.SharedMainParts;
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.io.IOException;
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;
@@ -22,49 +18,50 @@ import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
*
* @author rschoene - Initial contribution
*/
-public class MainB {
- 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);
+public class MainB extends SharedMainParts<MqttHandler, WorldModelB> {
+ private final String TOPIC_ROBOT_REACHABILITY;
+ 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;
+
+ MainB(String configFile) {
+ super("place-b", UtilB.pathToDirectoryOfPlaceB().resolve(configFile));
+ this.TOPIC_ROBOT_REACHABILITY = cellName + "/reachability/%s";
+ 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";
}
- private void run(String[] args) throws Exception {
+ public static void main(String[] args) throws Exception {
String configFile = args.length == 0 ? "src/main/resources/config-b.yaml" : args[0];
+ new MainB(configFile).run();
+ }
- // --- 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)));
+ @Override
+ protected void createSpecificMainHandlerConnections() {
+ mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
+ UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed")
+ );
+ }
+ @Override
+ protected de.tudresden.inf.st.ceti.Scene readSceneAndRobots() throws Exception {
/// 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)));
+ for (String name : Util.extractRobotNames(scene)) {
+ Robot robot = UtilB.createRobot(name);
+ model.addRobot(robot);
+ // assumption: robots do not change during runtime, so we have stable connections
+ robot.connectCanReachObjectOfInterestWrapper(mqttUri(String.format(TOPIC_ROBOT_REACHABILITY, name), config));
+ }
/// Set (dummy) reachability, if configured
if (config.useReachability) {
@@ -79,41 +76,29 @@ public class MainB {
}
}
- /// Setup model connection
- model.ragconnectCheckIncremental();
- model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
+ return scene;
+ }
- /// Connect endpoints
+ @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));
- 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));
+ model.connectOtherScene1(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config));
+ model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false);
+ }
- exitCondition.await();
+ @Override
+ protected MqttHandler createMqttHandler() {
+ return new MqttHandler("mainHandlerB");
}
- 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));
- }
+ @Override
+ protected WorldModelB createWorldModel() {
+ return new WorldModelB();
}
- private void close() {
- logger.info("Exiting ...");
- mainHandler.close();
- model.ragconnectCloseConnections();
+ @Override
+ protected String getModelInfos(WorldModelB worldModelB, boolean detailed) {
+ return UtilB.getModelInfos(model, detailed);
}
}
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 20b29d23ca5e2cb583a4e6ae15616304d26507c4..b8023e726595aff10197fecbb8d07bc1850b3e5e 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,17 +1,21 @@
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.Configuration;
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.List;
+import java.util.concurrent.TimeUnit;
+import java.util.stream.Collectors;
+
+import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
+import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
/**
* Testing features for placeB.
@@ -30,10 +34,183 @@ public class SimpleMainB {
private void run(String[] args) throws Exception {
// testBuildModelB();
// testReceivingModelB();
- testReachability();
+// testReachability();
+// testWeirdBehaviour();
+// testReachability();
+// testBFS();
+ testRobotReachabilityBFS();
+ }
+
+ private void testRobotReachabilityBFS() throws Exception {
+ WorldModelB model = new WorldModelB();
+ de.tudresden.inf.st.ceti.Scene scene = readScene(
+ UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json")
+ );
+ Scene myScene = UtilB.convert(scene);
+ model.setMyScene(myScene);
+ for (String name : Util.extractRobotNames(scene)) {
+ Robot robot = UtilB.createRobot(name);
+ model.addRobot(robot);
+ String filenameReachability = "src/main/resources/dummy-reachability-b-" + name + ".json";
+ Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(filenameReachability));
+ Reachability reachability = UtilB.readReachability(path);
+ CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability);
+ robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper);
+ }
+
+ printReachability(model);
+ printShortestPath(model, "binRed", "binGreen");
+ printShortestPath(model, "binBlue", "binYellow");
+ printShortestPath(model, "binRed", "binPurple");
+ }
+
+ private void printReachability(WorldModelB model) {
+ System.out.println("ModelInfos:");
+ System.out.println(UtilB.getModelInfos(model, true));
+
+ System.out.println("Reachability:");
+ model.getRobotList().forEach(r -> System.out.println(r.getName() + ": " + r.reachableObjects().stream().map(LogicalObjectOfInterest::getName).collect(Collectors.joining(", "))));
+
+ System.out.println("ReachabilityGraph:");
+ System.out.println(model.toReachabilityGraph().prettyPrint());
+ }
+
+ private void printShortestPath(WorldModelB model, String source, String target) {
+ LogicalDropOffLocation sourceLocation = model.getMyScene().getLogicalScene()
+ .resolveLogicalObjectOfInterest(source).asLogicalDropOffLocation();
+ LogicalDropOffLocation targetLocation = model.getMyScene().getLogicalScene()
+ .resolveLogicalObjectOfInterest(target).asLogicalDropOffLocation();
+
+ List<Edge> bfs = sourceLocation.correspondingVertex().orElseThrow().BFS(targetLocation.correspondingVertex().orElseThrow());
+
+ System.out.println("Shortest path from " + sourceLocation.getName() + " to " + targetLocation.getName() + ": " + bfs);
+ }
+
+ private void testBFS() {
+ /*
+ A B
+ | \ |
+ C - D
+ */
+ Graph g = new Graph();
+ Vertex a = makeVertex("a");
+ Vertex b = makeVertex("b");
+ Vertex c = makeVertex("c");
+ Vertex d = makeVertex("d");
+ Edge ac = makeEdge(a, c);
+ Edge ad = makeEdge(a, d);
+ Edge cd = makeEdge(c, d);
+ Edge db = makeEdge(d, b);
+ g.addVertex(a);
+ g.addVertex(b);
+ g.addVertex(c);
+ g.addVertex(d);
+ g.addEdge(ac);
+ g.addEdge(ad);
+ g.addEdge(cd);
+ g.addEdge(db);
+
+ System.out.println(a.BFS(b));
+ }
+
+ private Vertex makeVertex(String name) {
+ return new Vertex() {
+ @Override
+ public String toString() {
+ return name;
+ }
+ };
+ }
+
+ private Edge makeEdge(Vertex from, Vertex to) {
+ Edge result = new Edge();
+ result.setFrom(from);
+ result.setTo(to);
+ return result;
+ }
+
+ private void testDistance() throws Exception {
+ de.tudresden.inf.st.ceti.Scene scene = readScene(
+ UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json")
+ );
+
+ Scene myScene = UtilB.convert(scene);
+ float arm1X = 0f;
+ float arm1Y = 0f;
+ float arm1Z = 0.75f;
+
+ float arm2X = 1f;
+ float arm2Y = 0f;
+ float arm2Z = 0.75f;
+
+ for (var location : myScene.getDropOffLocationList()) {
+ check(arm1X, arm1Y, arm1Z, location.getPosition(), "arm1", location.getName());
+ check(arm2X, arm2Y, arm2Z, location.getPosition(), "arm2", location.getName());
+ }
+ }
+
+ private void check(float x, float y, float z, Position position, String robotName, String locationName) {
+ float dx = x - position.getX();
+ float dy = y - position.getY();
+ float dz = z - position.getZ();
+
+ if (Math.sqrt(dx*dx + dy*dy) < 0.05) {
+ System.out.println(robotName + " too close to " + locationName);
+ }
+ if (Math.sqrt(dx*dx + dy*dy + dz*dz) > 0.75) {
+ System.out.println(robotName + " too far away from " + locationName);
+ }
+ }
+
+ private void testWeirdBehaviour() throws IOException {
+ WorldModelB model = new WorldModelB();
+ Scene scene = new Scene();
+
+ MovableObject obj1 = new MovableObject();
+ obj1.setPosition(Position.of(1, 1, 1));
+ obj1.setOrientation(Orientation.of(0, 0, 0, 0));
+ obj1.setSize(Size.of(1, 1, 1));
+ scene.addMovableObject(obj1);
+
+ DropOffLocation loc = new DropOffLocation();
+ loc.setPosition(Position.of(1, 1, 1));
+ loc.setOrientation(Orientation.of(0, 0, 0, 0));
+ loc.setSize(Size.of(1, 1, 1));
+ scene.addDropOffLocation(loc);
+
+ model.setMyScene(scene);
+
+ Configuration config = new Configuration();
+ config.mqttHost = "localhost";
+ /// Setup model connection
+ model.ragconnectCheckIncremental();
+ model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS);
+
+ /// Connect endpoints
+ model.connectMyScene(mqttUri("TOPIC_MY_SCENE_UPDATE_FROM_ROS", config));
+ model.connectOtherScene1(mqttUri("TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A", config));
+ model.connectNextOperation(mqttUri("TOPIC_COMMANDS", config), false);
+
+ System.out.println("before");
+ System.out.println(scene.prettyPrint());
+
+ obj1.dumpDependencies();
+
+ Position evilPosition = Position.of(99, 99, 99);
+ obj1.setPosition(evilPosition);
+
+ System.out.println("after");
+ obj1.dumpDependencies();
+
+ System.out.println(scene.prettyPrint());
+ System.out.println(scene.prettyPrint());
+
+ obj1.dumpDependencies();
+
+ model.ragconnectCloseConnections();
}
- private void testReachability() throws Exception {
+ private void testDummyReachability() throws Exception {
WorldModelB model = new WorldModelB();
Robot arm1 = UtilB.createRobot("ARM1");
model.addRobot(arm1);
@@ -87,7 +264,7 @@ public class SimpleMainB {
LogicalDropOffLocation otherGamma = new LogicalDropOffLocation().setName("placeGamma");
otherGamma.addContainedObject(otherObj1);
otherScene.addLogicalDropOffLocation(otherGamma);
- model.setOtherScene(otherScene);
+ model.setOtherScene1(otherScene);
// printing and testing
printModelInfos(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
index 80b67ac249e4fefbae14e04e19d965c63a6877b2..64b88e1562b3a5b6739aaefad95214e0ca3d2119 100644
--- 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
@@ -1,7 +1,6 @@
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;
@@ -9,12 +8,8 @@ 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.
@@ -47,12 +42,73 @@ public class UtilB {
return result;
}
+ static void updatePositionOfObjectToLocation(Scene scene, String objName, String locationName) {
+ ObjectOfInterest obj = scene.resolveObjectOfInterest(objName);
+ ObjectOfInterest location = scene.resolveObjectOfInterest(locationName);
+ if (obj != null && location != null) {
+ // move objectRed1 to binBlue
+ logger.info("Got " + obj + " and location " + location);
+ logger.debug("before to {} at {}\n{}", locationName, location.getPosition(),
+ scene.prettyPrint());
+ logger.debug("X={}, pos.prettyPrint={}, obj.prettyPrint={}",
+ obj.getPosition().getX(),
+ obj.getPosition().prettyPrint(),
+ ((MovableObject) obj).prettyPrint());
+
+ obj.dumpDependencies();
+
+ obj.setPosition(Position.of(location.getPosition().getX(),
+ location.getPosition().getY(),
+ location.getPosition().getZ()));
+
+ logger.debug("after");
+ obj.dumpDependencies();
+
+ logger.debug(scene.prettyPrint());
+ logger.debug("X={}, pos.prettyPrint={}, obj.prettyPrint={}",
+ obj.getPosition().getX(),
+ obj.getPosition().prettyPrint(),
+ ((MovableObject) obj).prettyPrint());
+
+ obj.dumpDependencies();
+ } else {
+ logger.error("Obj (" + obj + ") or location (" + location + ") are null");
+ }
+ }
+
+ private static String resolveObjName(WorldModelB model, String objName) {
+ if (!model.hasMyScene()) {
+ return "\"" + objName + "\"";
+ }
+ try {
+ return model.getMyScene().getLogicalScene().resolveLogicalObjectOfInterest(objName).toString();
+ } catch (NullPointerException ignore) {
+ return "+" + objName + "(not resolved)+";
+ }
+ }
+
static String getModelInfos(WorldModelB model) {
+ return getModelInfos(model, false);
+ }
+
+ static String getModelInfos(WorldModelB model, boolean detailed) {
StringBuilder sb = new StringBuilder();
sb.append("myRobots: ")
.append(model.getRobotList().prettyPrint(
- robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList().prettyPrint(CanReachObjectOfInterest::getObjectName) + ")"))
+ robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper()
+ .getCanReachObjectOfInterestList()
+ .prettyPrint(canReachObj -> resolveObjName(model, canReachObj.getObjectName()))
+ + ")"))
.append("\n");
+ if (detailed) {
+ // also include "normal" scene
+ sb.append("myScene:");
+ if (model.hasMyScene()) {
+ sb.append("\n").append(model.getMyScene().prettyPrint());
+ } else {
+ sb.append(" (unset)\n");
+ }
+ }
sb.append("myLogicalScene:");
if (model.hasMyScene()) {
sb.append("\n").append(model.getMyScene().getLogicalScene().prettyPrint());
@@ -61,13 +117,13 @@ public class UtilB {
}
sb.append("otherScene:");
if (model.hasOtherScene()) {
- sb.append("\n").append(model.getOtherScene().prettyPrint());
+ sb.append("\n").append(model.mergedOtherScene().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");
+ sb.append("Diff: ").append(model.diffScenes().prettyPrint(Difference::prettyPrint)).append("\n");
+ sb.append("Operations: ").append(model.diffToOperations().prettyPrint(Operation::prettyPrint)).append("\n");
+ sb.append("Next operation: ").append(model.getNextOperation().prettyPrint()).append("\n");
return sb.toString();
}
@@ -82,10 +138,10 @@ public class UtilB {
@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);
+ return ASTNode._ragconnect__apply_ConvertScene(pbScene);
}
public CanReachObjectOfInterestWrapper exposed_apply_ConvertReachability(de.tudresden.inf.st.ceti.Reachability r) throws Exception {
- return ASTNode._apply_ConvertReachability(r);
+ return ASTNode._ragconnect__apply_ConvertReachability(r);
}
}
diff --git a/ros3rag.placeB/src/main/resources/config-b.yaml b/ros3rag.placeB/src/main/resources/config-b.yaml
index 91aa9417a2e917f521e4a87944d9f96efd78cb34..657f5ba3915b109991428609720b061f7aa45f12 100644
--- a/ros3rag.placeB/src/main/resources/config-b.yaml
+++ b/ros3rag.placeB/src/main/resources/config-b.yaml
@@ -1,8 +1,11 @@
mqttHost: "localhost"
filenameInitialScene: "src/main/resources/config-scene-b.json"
-useReachability: true
+useReachability: false
reachability:
- idRobot: "arm1"
filename: "src/main/resources/dummy-reachability-b-arm1.json"
- idRobot: "arm2"
filename: "src/main/resources/dummy-reachability-b-arm2.json"
+ - idRobot: "arm3"
+ filename: "src/main/resources/dummy-reachability-b-arm3.json"
+coordinatorMqttTopicPrefix: "coordinating/rag-b"
diff --git a/ros3rag.placeB/src/main/resources/config-scene-b.json b/ros3rag.placeB/src/main/resources/config-scene-b.json
index f14e17fe05da23db662d514b49ae47d406a0ddd4..7cec4ff886a6b3c30d5e97f135fbaf5def049061 100644
--- a/ros3rag.placeB/src/main/resources/config-scene-b.json
+++ b/ros3rag.placeB/src/main/resources/config-scene-b.json
@@ -4,18 +4,23 @@
{ "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": "binBlue","type": "DROP_OFF_LOCATION","pos": { "x": 1.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": "DROP_OFF_LOCATION","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": "DROP_OFF_LOCATION","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": "binYellow","type": "DROP_OFF_LOCATION","pos": { "x": 1.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "g": 1 } },
+ { "id": "binPurple","type": "DROP_OFF_LOCATION","pos": { "x": 2.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1, "b": 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": "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": 0.382683,"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": "objectYellow1","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, "g": 1 } },
+ { "id": "objectPurple1","type": "BOX","pos": { "x": 0.3,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1, "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 } }
+ { "id": "arm2","type": "ARM","pos": { "x": 1, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } },
+ { "id": "arm3","type": "ARM","pos": { "x": 2, "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-arm3.json b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm3.json
new file mode 100644
index 0000000000000000000000000000000000000000..9e8b59b043810181586d8b2515f46a43d8d3f56f
--- /dev/null
+++ b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm3.json
@@ -0,0 +1,13 @@
+{
+ "idRobot": "ARM3",
+ "objects": [
+ { "idObject": "objectRed1", "reachable": true },
+ { "idObject": "objectRed2", "reachable": true },
+ { "idObject": "objectRed3", "reachable": false },
+ { "idObject": "binRed", "reachable": false },
+ { "idObject": "binBlue", "reachable": false },
+ { "idObject": "binGreen", "reachable": true },
+ { "idObject": "binYellow", "reachable": false },
+ { "idObject": "binPurple", "reachable": true }
+ ]
+}
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java
new file mode 100644
index 0000000000000000000000000000000000000000..fe53722780e481e0c6c086de7e4c4374205cea1d
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java
@@ -0,0 +1,175 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+import java.util.List;
+
+import static de.tudresden.inf.st.placeB.TestUtils.*;
+import static de.tudresden.inf.st.placeB.TestUtils.addMyObjects;
+import static org.assertj.core.api.Assertions.assertThat;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+/**
+ * Testing computing operations from differences.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestComputeOperations {
+
+ @ParameterizedTest(name = "testNoMovePossible [emptyPreviousLocation = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void testNoMovePossible(boolean emptyPreviousLocation) {
+ WorldModelB model = newModel();
+ addMyObjects(model, "x");
+ addMyLocations(model, "red", "blue", "green");
+ addRobots(model, "arm1", "arm2");
+ addReachability(model, "arm1", "red", "blue", "x");
+ addReachability(model, "arm2", "blue");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ DifferenceObjectAtWrongPlace diff = createDifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, green);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(1);
+ Operation op = operations.get(0);
+ assertTrue(op.isErrorOperation());
+ assertEquals(emptyPreviousLocation ?
+ "+Error: No sequence of operations to move x to green+" :
+ "+Error: No sequence of operations to move x from red to green+", op.prettyPrint());
+ }
+
+ @ParameterizedTest(name = "testDirectMove [emptyPreviousLocation = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void testDirectMove(boolean emptyPreviousLocation) {
+ WorldModelB model = newModel();
+ addMyObjects(model, "x");
+ addMyLocations(model, "red", "blue", "green");
+ addRobots(model, "arm1", "arm2");
+ addReachability(model, "arm1", "red", "blue", "x");
+ addReachability(model, "arm2", "blue");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+ Robot arm1 = model.findRobot("arm1").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = createDifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, blue);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(1);
+ Operation op = operations.get(0);
+ assertThat(op).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace = (PickAndPlace) op;
+ assertEquals(arm1, pickAndPlace.getRobotToExecute());
+ assertEquals(x, pickAndPlace.getObjectToPick());
+ assertEquals(blue, pickAndPlace.getTargetLocation());
+ }
+
+ @ParameterizedTest(name = "testIndirectMoveWith2Robots [emptyPreviousLocation = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void testIndirectMoveWith2Robots(boolean emptyPreviousLocation) {
+ WorldModelB model = newModel();
+ addMyObjects(model, "x");
+ addMyLocations(model, "red", "blue", "green");
+ addRobots(model, "arm1", "arm2");
+ addReachability(model, "arm1", "red", "blue", "x");
+ addReachability(model, "arm2", "blue", "green");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ Robot arm1 = model.findRobot("arm1").orElseThrow();
+ Robot arm2 = model.findRobot("arm2").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = createDifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, green);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(2);
+ Operation op1 = operations.get(0);
+ assertThat(op1).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+ assertEquals(arm1, pickAndPlace1.getRobotToExecute());
+ assertEquals(x, pickAndPlace1.getObjectToPick());
+ assertEquals(blue, pickAndPlace1.getTargetLocation());
+
+ Operation op2 = operations.get(1);
+ assertThat(op2).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+ assertEquals(arm2, pickAndPlace2.getRobotToExecute());
+ assertEquals(x, pickAndPlace2.getObjectToPick());
+ assertEquals(green, pickAndPlace2.getTargetLocation());
+ }
+
+ @ParameterizedTest(name = "testIndirectMoveWith3Robots [emptyPreviousLocation = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void testIndirectMoveWith3Robots(boolean emptyPreviousLocation) {
+ /*
+ * red -(arm1)-> blue --(arm3)-,
+ * `-(arm2)-> green <-------'
+ * `--(arm4)-> yellow -(arm5)-> purple
+ */
+ WorldModelB model = newModel();
+ addMyObjects(model, "x");
+ addMyLocations(model, "red", "blue", "green", "yellow", "purple");
+ addRobots(model, "arm1", "arm2", "arm3", "arm4", "arm5");
+ addReachability(model, "arm1", "red", "blue", "x");
+ addReachability(model, "arm2", "red", "green", "x");
+ addReachability(model, "arm3", "blue", "green");
+ addReachability(model, "arm4", "green", "yellow");
+ addReachability(model, "arm5", "yellow", "purple");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ LogicalDropOffLocation yellow = logicalScene.resolveLogicalObjectOfInterest("yellow").asLogicalDropOffLocation();
+ LogicalDropOffLocation purple = logicalScene.resolveLogicalObjectOfInterest("purple").asLogicalDropOffLocation();
+ Robot arm2 = model.findRobot("arm2").orElseThrow();
+ Robot arm4 = model.findRobot("arm4").orElseThrow();
+ Robot arm5 = model.findRobot("arm5").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = createDifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, purple);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(3);
+
+ Operation op1 = operations.get(0);
+ assertThat(op1).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+ assertEquals(arm2, pickAndPlace1.getRobotToExecute());
+ assertEquals(x, pickAndPlace1.getObjectToPick());
+ assertEquals(green, pickAndPlace1.getTargetLocation());
+
+ Operation op2 = operations.get(1);
+ assertThat(op2).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+ assertEquals(arm4, pickAndPlace2.getRobotToExecute());
+ assertEquals(x, pickAndPlace2.getObjectToPick());
+ assertEquals(yellow, pickAndPlace2.getTargetLocation());
+
+ Operation op3 = operations.get(2);
+ assertThat(op3).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace3 = (PickAndPlace) op3;
+ assertEquals(arm5, pickAndPlace3.getRobotToExecute());
+ assertEquals(x, pickAndPlace3.getObjectToPick());
+ assertEquals(purple, pickAndPlace3.getTargetLocation());
+ }
+
+ private DifferenceObjectAtWrongPlace createDifferenceObjectAtWrongPlace(LogicalMovableObject x, LogicalDropOffLocation previousLocation, LogicalDropOffLocation newLocation) {
+ DifferenceObjectAtWrongPlace result = new DifferenceObjectAtWrongPlace();
+ result.setObject(x);
+ result.setPreviousLocation(previousLocation);
+ result.setNewLocation(newLocation);
+ return result;
+ }
+}
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java
new file mode 100644
index 0000000000000000000000000000000000000000..fb2ffaae2f255e8e5ffd7cc5210d365e5864743b
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java
@@ -0,0 +1,190 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+import org.junit.jupiter.api.Test;
+
+import static de.tudresden.inf.st.placeB.TestUtils.*;
+import static org.assertj.core.api.Assertions.assertThat;
+import static org.assertj.core.api.Assertions.tuple;
+import static org.junit.jupiter.api.Assertions.*;
+
+/**
+ * Testing computing differences.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestDifference {
+
+ @Test
+ public void testDifferenceObjectAtWrongPlaceNoPreviousLocation() {
+ // myScene: object (x) not contained in any location
+ // targetScene: object (x) in location (red)
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(4, 4, 4), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+ addContainedObjects(model, "red", "x");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ assertFalse(x.hasLocatedAt());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertEquals(1, diffs.getNumChild());
+ Difference diff = diffs.getChild(0);
+ assertThat(diff).isInstanceOf(DifferenceObjectAtWrongPlace.class);
+ DifferenceObjectAtWrongPlace wrongPlace = (DifferenceObjectAtWrongPlace) diff;
+ assertEquals("x", wrongPlace.getObject().getName());
+ assertNull(wrongPlace.getPreviousLocation());
+ assertEquals("red", wrongPlace.getNewLocation().getName());
+ }
+
+ @Test
+ public void testDifferenceObjectAtWrongPlaceOtherPreviousLocation() {
+ // myScene: object (x) contained in another location (blue)
+ // targetScene: object (x) in location (red)
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "blue", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+ addContainedObjects(model, "red", "x");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ assertEquals("blue", x.getLocatedAt().getName());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertEquals(1, diffs.getNumChild());
+ Difference diff = diffs.getChild(0);
+ assertThat(diff).isInstanceOf(DifferenceObjectAtWrongPlace.class);
+ DifferenceObjectAtWrongPlace wrongPlace = (DifferenceObjectAtWrongPlace) diff;
+ assertEquals("x", wrongPlace.getObject().getName());
+ assertEquals("blue", wrongPlace.getPreviousLocation().getName());
+ assertEquals("red", wrongPlace.getNewLocation().getName());
+ }
+
+ @Test
+ public void testDifferenceObjectMisplaced() {
+ // myScene: object (x) contained in a location (red)
+ // targetScene: object (x) not contained in any location
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ assertEquals("red", x.getLocatedAt().getName());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertEquals(1, diffs.getNumChild());
+ Difference diff = diffs.getChild(0);
+ assertThat(diff).isInstanceOf(DifferenceObjectMisplaced.class);
+ DifferenceObjectMisplaced misplaced = (DifferenceObjectMisplaced) diff;
+ assertEquals("x", misplaced.getObject().getName());
+ assertEquals("red", misplaced.getPreviousLocation().getName());
+ }
+
+ @Test
+ public void testNoDifferenceNoContainment() {
+ // myScene: object (x) not contained in any location
+ // targetScene: object (x) not contained in any location
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(8, 8, 8), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ assertFalse(x.hasLocatedAt());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertThat(diffs).isEmpty();
+ }
+
+ @Test
+ public void testNoDifferenceSameLocation() {
+ // myScene: object (x) contained in same location (red)
+ // targetScene: object (x) contained in a location (red)
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+ addContainedObjects(model, "red", "x");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ assertEquals("red", x.getLocatedAt().getName());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertThat(diffs).isEmpty();
+ }
+
+ @Test
+ public void testDifferenceNewObject() {
+ // myScene: object (x) not known
+ // targetScene: object (x) exists, not contained in any location
+ WorldModelB model = newModel();
+ addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x");
+ addOtherLocations(model, "red");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ assertNull(myLogicalScene.resolveLogicalObjectOfInterest("x"));
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertEquals(1, diffs.getNumChild());
+ Difference diff = diffs.getChild(0);
+ assertThat(diff).isInstanceOf(DifferenceNewObject.class);
+ DifferenceNewObject diffNewObject = (DifferenceNewObject) diff;
+ assertEquals("x", diffNewObject.getObject().getName());
+ }
+
+ @Test
+ public void testMultipleDifferenceObjectAtWrongPlace() {
+ // myScene: objects (x, y) contained in other locations (blue, green)
+ // targetScene: objects (x, y) in location (red)
+ WorldModelB model = newModel();
+ addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyObject(model, "y", new Position(2.5f, 2.5f, 2.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f));
+ addMyLocation(model, "blue", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1));
+ addMyLocation(model, "green", new Position(2, 2, 2), noRotation(), new Size(1, 1, 1));
+
+ addOtherObjects(model, "x", "y");
+ addOtherLocations(model, "red");
+ addContainedObjects(model, "red", "x", "y");
+
+ LogicalScene myLogicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalMovableObject y = myLogicalScene.resolveLogicalObjectOfInterest("y").asLogicalMovableObject();
+ assertEquals("blue", x.getLocatedAt().getName());
+ assertEquals("green", y.getLocatedAt().getName());
+
+ JastAddList<Difference> diffs = model.diffScenes();
+ assertNotNull(diffs);
+ assertEquals(2, diffs.getNumChild());
+ assertThat(diffs).allMatch(d -> d instanceof DifferenceObjectAtWrongPlace);
+ //noinspection unchecked
+ assertThat(diffs).map(diff -> (DifferenceObjectAtWrongPlace) diff)
+ .extracting(d -> d.getObject().getName(), d -> d.getPreviousLocation().getName(), d -> d.getNewLocation().getName())
+ .containsOnly(tuple("x", "blue", "red"),
+ tuple("y", "green", "red"));
+ }
+}
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestMultiScenes.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestMultiScenes.java
new file mode 100644
index 0000000000000000000000000000000000000000..dc1080a5f69832879c19ba4ad849c729fdcf9776
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestMultiScenes.java
@@ -0,0 +1,142 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.LogicalDropOffLocation;
+import de.tudresden.inf.st.placeB.ast.LogicalMovableObject;
+import de.tudresden.inf.st.placeB.ast.LogicalScene;
+import de.tudresden.inf.st.placeB.ast.WorldModelB;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+import java.util.Collections;
+
+import static de.tudresden.inf.st.placeB.TestUtils.newModel;
+import static org.assertj.core.api.Assertions.assertThat;
+
+/**
+ * Testing merging of multiple (other) scenes.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestMultiScenes {
+
+ @Test
+ public void noOtherScenes() {
+ WorldModelB model = newModel();
+
+ LogicalScene actual = model.mergedOtherScene();
+
+ assertThat(actual.getLogicalDropOffLocationList()).isEmpty();
+ assertThat(actual.getLogicalMovableObjectList()).isEmpty();
+ }
+
+ @ParameterizedTest(name = "twoScenesContains [scene1 = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void oneSceneNoContains(boolean scene1) {
+ WorldModelB model = newModel();
+ TestUtils.addOtherLocations(scene1 ? model.getOtherScene1() : model.getOtherScene2(), "red");
+ TestUtils.addOtherObjects(scene1 ? model.getOtherScene1() : model.getOtherScene2(), "a");
+
+ LogicalScene actual = model.mergedOtherScene();
+
+ assertThat(actual.getLogicalDropOffLocationList())
+ .extracting("Name").containsOnly("red");
+ assertThat(actual.getLogicalMovableObjectList())
+ .extracting("Name").containsOnly("a");
+ }
+
+ @Test
+ public void twoScenesNoContains() {
+ WorldModelB model = newModel();
+ TestUtils.addOtherLocations(model, "red");
+ TestUtils.addOtherObjects(model, "a");
+ TestUtils.addOtherLocations(model.getOtherScene2(), "blue");
+ TestUtils.addOtherObjects(model.getOtherScene2(), "b");
+
+ LogicalScene actual = model.mergedOtherScene();
+
+ assertThat(actual.getLogicalDropOffLocationList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene2().getLogicalDropOffLocationList())
+ .extracting("Name").containsOnly("red", "blue");
+ assertThat(actual.getLogicalDropOffLocationList())
+ .extracting("ContainedObjects").containsOnly(Collections.emptyList());
+
+ assertThat(actual.getLogicalMovableObjectList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalMovableObjectList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalMovableObjectList())
+ .doesNotContainAnyElementsOf(model.getOtherScene2().getLogicalMovableObjectList())
+ .extracting("Name").containsOnly("a", "b");
+ assertThat(actual.getLogicalMovableObjectList())
+ .extracting("LocatedAt").containsOnly((LogicalDropOffLocation) null);
+ }
+
+ @ParameterizedTest(name = "twoScenesContains [scene1 = {argumentsWithNames}]")
+ @ValueSource(booleans = {true, false})
+ public void twoScenesContains1(boolean scene1) {
+ WorldModelB model = newModel();
+ TestUtils.addOtherLocations(model, "red", "blue");
+ TestUtils.addOtherObjects(model, "a", "b");
+ model.getOtherScene2().addLogicalDropOffLocation(new LogicalDropOffLocation().setName("red"));
+ model.getOtherScene2().addLogicalMovableObject(new LogicalMovableObject().setName("a"));
+ TestUtils.addContainedObjects(scene1 ? model.getOtherScene1() : model.getOtherScene2(),
+ "red", "a");
+
+ LogicalScene actual = model.mergedOtherScene();
+
+ assertThat(actual.getLogicalDropOffLocationList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene2().getLogicalDropOffLocationList())
+ .extracting("Name").containsExactly("red", "blue");
+ LogicalDropOffLocation red = actual.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ assertThat(red.getContainedObjectList())
+ .extracting("Name").containsOnly("a");
+
+ assertThat(actual.getLogicalMovableObjectList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalMovableObjectList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalMovableObjectList())
+ .extracting("Name").containsOnly("a", "b");
+ LogicalMovableObject a = actual.resolveLogicalObjectOfInterest("a").asLogicalMovableObject();
+ assertThat(a.getLocatedAt())
+ .extracting("Name").isEqualTo("red");
+ }
+
+ @Test
+ public void twoScenesRedContainsATwice() {
+ WorldModelB model = newModel();
+ TestUtils.addOtherLocations(model, "red", "blue");
+ TestUtils.addOtherObjects(model, "a", "b");
+ TestUtils.addContainedObjects(model, "red", "a");
+
+ TestUtils.addOtherLocations(model.getOtherScene2(), "red");
+ TestUtils.addOtherObjects(model.getOtherScene2(), "a", "b");
+ TestUtils.addContainedObjects(model.getOtherScene2(), "red", "a");
+
+ LogicalScene actual = model.mergedOtherScene();
+
+ assertThat(actual.getLogicalDropOffLocationList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalDropOffLocationList())
+ .doesNotContainAnyElementsOf(model.getOtherScene2().getLogicalDropOffLocationList())
+ .extracting("Name").containsExactly("red", "blue");
+ LogicalDropOffLocation red = actual.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ assertThat(red.getContainedObjectList())
+ .extracting("Name").containsOnly("a");
+
+ assertThat(actual.getLogicalMovableObjectList())
+ .size().isEqualTo(2);
+ assertThat(actual.getLogicalMovableObjectList())
+ .doesNotContainAnyElementsOf(model.getOtherScene1().getLogicalMovableObjectList())
+ .extracting("Name").containsOnly("a", "b");
+ LogicalMovableObject a = actual.resolveLogicalObjectOfInterest("a").asLogicalMovableObject();
+ assertThat(a.getLocatedAt())
+ .extracting("Name").isEqualTo("red");
+ }
+}
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java
new file mode 100644
index 0000000000000000000000000000000000000000..6e5f8fe77601bf88ef82f6f3ef0764c60e932e7c
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java
@@ -0,0 +1,103 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+
+/**
+ * Utility methods for testing.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestUtils {
+ public static WorldModelB newModel() {
+ WorldModelB model = new WorldModelB();
+ Scene scene = new Scene();
+ model.setMyScene(scene);
+ model.setOtherScene1(new LogicalScene());
+ model.setOtherScene2(new LogicalScene());
+ return model;
+ }
+
+ public static Orientation noRotation() {
+ return new Orientation().setW(1);
+ }
+
+ public static void addMyObjects(WorldModelB model, String... names) {
+ for (String name : names) {
+ int index = model.getMyScene().getNumMovableObject();
+ addMyObject(model, name, new Position(-index, -index, -index), new Orientation(), new Size(1, 1, 1));
+ }
+ }
+
+ public static void addMyObject(WorldModelB model, String name, Position position, Orientation orientation, Size size) {
+ MovableObject obj = new MovableObject()
+ .setName(name)
+ .setPosition(position)
+ .setOrientation(orientation)
+ .setSize(size);
+ model.getMyScene().addMovableObject(obj);
+ }
+
+ public static void addMyLocations(WorldModelB model, String... names) {
+ for (String name : names) {
+ int index = model.getMyScene().getNumDropOffLocation();
+ addMyLocation(model, name, new Position(+index, +index, +index), new Orientation(), new Size(1, 1, 1));
+ }
+ }
+
+ public static void addMyLocation(WorldModelB model, String name, Position position, Orientation orientation, Size size) {
+ DropOffLocation obj = new DropOffLocation()
+ .setName(name)
+ .setPosition(position)
+ .setOrientation(orientation)
+ .setSize(size);
+ model.getMyScene().addDropOffLocation(obj);
+ }
+
+ public static void addRobots(WorldModelB model, String... names) {
+ for (String name : names) {
+ Robot obj = new Robot().setName(name);
+ model.addRobot(obj);
+ }
+ }
+
+ public static void addReachability(WorldModelB model, String robotName, String... reachableObjects) {
+ Robot robot = model.findRobot(robotName).orElseThrow();
+ CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper();
+ for (String reachableObjectName : reachableObjects) {
+ wrapper.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(reachableObjectName));
+ }
+ robot.setCanReachObjectOfInterestWrapper(wrapper);
+ }
+
+ public static void addOtherObjects(WorldModelB model, String... names) {
+ addOtherObjects(model.getOtherScene1(), names);
+ }
+
+ public static void addOtherObjects(LogicalScene scene, String... names) {
+ for (String name : names) {
+ scene.addLogicalMovableObject(new LogicalMovableObject().setName(name));
+ }
+ }
+
+ public static void addOtherLocations(WorldModelB model, String... names) {
+ addOtherLocations(model.getOtherScene1(), names);
+ }
+
+ public static void addOtherLocations(LogicalScene scene, String... names) {
+ for (String name : names) {
+ scene.addLogicalDropOffLocation(new LogicalDropOffLocation().setName(name));
+ }
+ }
+
+ public static void addContainedObjects(WorldModelB model, String locationName, String... objectNames) {
+ addContainedObjects(model.getOtherScene1(), locationName, objectNames);
+ }
+
+ public static void addContainedObjects(LogicalScene scene, String locationName, String... objectNames) {
+ LogicalDropOffLocation location = scene.resolveLogicalObjectOfInterest(locationName).asLogicalDropOffLocation();
+ for (String objectName : objectNames) {
+ location.addContainedObject(scene.resolveLogicalObjectOfInterest(objectName).asLogicalMovableObject());
+ }
+ }
+
+}
diff --git a/settings.gradle b/settings.gradle
index bd2c1a8453b44141ce617dbfeb3570e9786f6637..19ff0e6b692daa207124b6394d01c1dabf1c14f7 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -1,8 +1,11 @@
+pluginManagement {
+ plugins {
+ id 'org.jastadd' version "${jastadd_gradle_version}"
+ }
+}
+
rootProject.name = 'ros3rag'
include 'ros3rag.placeA'
include 'ros3rag.placeB'
include 'ros3rag.common'
-
-// include 'ros3rag.senderstub'
-// include 'ros3rag.receiverstub'