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/libs/ragconnect-0.3.1.jar b/libs/ragconnect-0.3.1.jar
index faabd7a763be13599c78fd851038b6ec69dbb79d..da8df316201d4ddede05a35e14d01f757df33582 100644
Binary files a/libs/ragconnect-0.3.1.jar and b/libs/ragconnect-0.3.1.jar differ
diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd
index ed44d6ae3c01d9886b549ad1652ee2430b7a4043..cc6d247bbfb5e25c316616634860747e2e685cf0 100644
--- a/ros3rag.common/src/main/resources/jastadd/types.jadd
+++ b/ros3rag.common/src/main/resources/jastadd/types.jadd
@@ -134,24 +134,38 @@ aspect Printing {
sb.append(" no locations\n");
} else {
for (var location : getDropOffLocationList()) {
- sb.append(" location ").append(location).append("\n");
+ sb.append(" location ").append(location.prettyPrint()).append("\n");
}
}
if (getNumMovableObject() == 0) {
sb.append(" no objects\n");
} else {
for (var obj : getMovableObjectList()) {
- sb.append(" obj ").append(obj).append("\n");
+ sb.append(" obj ").append(obj.prettyPrint()).append("\n");
}
}
return sb.toString();
}
- syn String MovableObject.toString() = "<obj " + getName() + getPosition() + getOrientation() + getSize() + "@" + Integer.toHexString(hashCode()) + ">";
- syn String DropOffLocation.toString() = "{loc " + getName() + getPosition() + getOrientation() + getSize() + "@" + Integer.toHexString(hashCode()) + "}";
- syn String Position.toString() = " pos=(" + getX() + "," + getY() + "," + getZ() + ")";
- syn String Orientation.toString() = " orient=(" + getX() + "," + getY() + "," + getZ() + "," + getW() + ")";
- syn String Size.toString() = " size=(" + getLength() + "," + getWidth() + "," + getHeight() + ")";
+ syn String MovableObject.prettyPrint() {
+ return "<obj " + getName() + getPosition().prettyPrint() + getOrientation().prettyPrint() + getSize().prettyPrint() + "@" + Integer.toHexString(hashCode()) + ">";
+ }
+
+ 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();
@@ -159,7 +173,7 @@ aspect Printing {
sb.append(" no locations\n");
} else {
for (var location : getLogicalDropOffLocationList()) {
- sb.append(" location ").append(location);
+ sb.append(" location ").append(location.prettyPrint());
if (!location.getContainedObjectList().isEmpty()) {
sb.append(" (objects: ").append(location.getContainedObjectList()).append(")");
}
@@ -170,7 +184,7 @@ aspect Printing {
sb.append(" no objects\n");
} else {
for (var obj : getLogicalMovableObjectList()) {
- sb.append(" obj ").append(obj);
+ sb.append(" obj ").append(obj.prettyPrint());
if (obj.hasLocatedAt()) {
sb.append(" (locatedAt: ").append(obj.getLocatedAt()).append(")");
}
@@ -180,8 +194,12 @@ aspect Printing {
return sb.toString();
}
- syn String LogicalMovableObject.toString() = "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">";
- syn String LogicalDropOffLocation.toString() = "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}";
+ syn String LogicalMovableObject.prettyPrint() {
+ return "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">";
+ }
+ syn String LogicalDropOffLocation.prettyPrint() {
+ return "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}";
+ }
}
aspect ConvenienceMethods {
diff --git a/ros3rag.common/src/main/resources/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle
index 7b7c51f49605ed3cfb0eaa059031c06ca15fb4f4..79b3eab1860edfbace31eb2fda88a3dd09f262e0 100644
--- a/ros3rag.common/src/main/resources/tasks.gradle
+++ b/ros3rag.common/src/main/resources/tasks.gradle
@@ -2,7 +2,8 @@
apply plugin: 'jastadd'
dependencies {
- jastadd2 "org.jastadd:jastadd:2.3.5"
+// jastadd2 "org.jastadd:jastadd:2.3.5"
+ jastadd2 fileTree(include: ['jastadd2.jar'], dir: '../libs')
api group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
}
@@ -27,7 +28,7 @@ task ragConnect(type: JavaExec) {
'--logWrites',
// '--verbose',
'--rootNode=' + project.ext.ragConnectRootNode,
- '--incremental=param,debug',
+ '--incremental=param',
"--tracing=cache,flush"
])
@@ -119,7 +120,7 @@ jastadd {
'--lineColumnNumbers',
'--List=JastAddList',
'--cache=all',
- "--flush=full",
+ "--flush=api",
"--incremental=param,debug",
"--tracing=cache,flush",
]
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 920de2346e4f68a7b904f91a5379cdad261e8415..c7cb57e9ec52e0574fad1f98e9afd71bd6c5dad7 100644
--- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
+++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java
@@ -1,6 +1,9 @@
package de.tudresden.inf.st.placeA;
-import de.tudresden.inf.st.placeA.ast.*;
+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.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
@@ -55,12 +58,12 @@ public class MainA {
mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
- mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_BLUE, bytes -> {
- updatePositionOfObjectToLocation("objectRed1", "binBlue");
- });
- mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes -> {
- updatePositionOfObjectToLocation("objectRed1", "binRed");
- });
+ 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")
+ );
/// Reading scene and robot
de.tudresden.inf.st.ceti.Scene scene = readScene(
@@ -93,25 +96,6 @@ public class MainA {
exitCondition.await();
}
- private void updatePositionOfObjectToLocation(String objName, String locationName) {
- ObjectOfInterest obj = model.getScene().resolveObjectOfInterest(objName);
- ObjectOfInterest location = model.getScene().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(),
- model.getScene().prettyPrint());
-
- obj.setPosition(Position.of(location.getPosition().getX(),
- location.getPosition().getY(),
- location.getPosition().getZ()));
-
- logger.debug("after\n{}", model.getScene().prettyPrint());
- } else {
- logger.error("Obj (" + obj + ") or location (" + location + ") are null");
- }
- }
-
private void setRobot(String name) {
if (model.getRobot() != null) {
logger.error("Robot already set. Overriding with newly found name.");
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 25721619b0b5aad42ac6bc0fd57600df56e11ea2..623867a147fbdef4627fdf5de7707de8690d26a2 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,6 +23,25 @@ public class UtilA {
return new ExposingASTNode().exposed_apply_ConvertScene(scene);
}
+ 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: ")
diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle
index 95d2d16ba73432095be516bff69b187c4fcd047f..1f16b72742b27666e4d19100b60d9eced53233a2 100644
--- a/ros3rag.placeB/build.gradle
+++ b/ros3rag.placeB/build.gradle
@@ -17,7 +17,6 @@ task simpleRun(type: JavaExec) {
group 'application'
classpath sourceSets.main.runtimeClasspath
main = "de.tudresden.inf.st.placeB.SimpleMainB"
-
}
dependencies {
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
index f77374792f0d58550ceb3515913b5a5a6f802752..3da080fb1a8643e37b1244231e7a5e92ca29d257 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect
@@ -7,5 +7,9 @@ receive tree Robot.CanReachObjectOfInterestWrapper using ParseReachability, Conv
send tree 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..0a63106de789c89b84ed8b808322c620287f67ae 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -1,4 +1,5 @@
aspect Computation {
+ syn nta LogicalDropOffLocation WorldModelB.unspecifiedLocation() = new LogicalDropOffLocation().setName("<unspecified>");
syn nta JastAddList<Difference> WorldModelB.diffScenes() {
var result = new JastAddList<Difference>();
if (!hasMyScene() || !hasOtherScene()) {
@@ -15,18 +16,29 @@ aspect Computation {
LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject();
if (myObject == null) {
// new object, currently not handled
- result.add(new DifferenceNewObject());
+ var diff = new DifferenceNewObject();
+ diff.setObject(otherObject);
+ result.add(diff);
continue;
}
- if (!myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName())) {
- // object at different locations
- var difference = new DifferenceObjectAtWrongPlace();
- difference.setObject(myObject);
- difference.setPreviousLocation(myObject.getLocatedAt());
- difference.setNewLocation(otherObject.getLocatedAt());
- result.add(difference);
+ if (myObject.hasLocatedAt()) {
+ if (!otherObject.hasLocatedAt()) {
+ var diff = new DifferenceObjectMisplaced();
+ diff.setObject(myObject);
+ diff.setPreviousLocation(myObject.getLocatedAt());
+ result.add(diff);
+ continue;
+ }
+ if (!myObject.getLocatedAt().getName().equals(otherObject.getLocatedAt().getName())) {
+ // object at different locations
+ var difference = new DifferenceObjectAtWrongPlace();
+ difference.setObject(myObject);
+ difference.setPreviousLocation(myObject.getLocatedAt());
+ difference.setNewLocation(otherObject.getLocatedAt());
+ result.add(difference);
+ }
}
- // other difference not handled yet, e.g. object deleted
+ // other difference not handled yet, e.g. object deleted, or objects having no locations
}
return result;
}
@@ -93,6 +105,10 @@ aspect Computation {
// FIXME. stub, may be implemented later
return Collections.emptyList();
}
+ eq DifferenceObjectMisplaced.computeOperations() {
+ // FIXME. stub, may be implemented later
+ return Collections.emptyList();
+ }
private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) {
var result = new PickAndPlace();
@@ -131,7 +147,8 @@ aspect Computation {
aspect AttributeMappings {
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!");
@@ -157,15 +174,26 @@ aspect GlueForShared {
}
aspect Printing {
- syn String Robot.toString() = "~Robot " + getName() + "~";
+ 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() + ": " + getPreviousLocation().prettyPrint() + " -> " + getNewLocation().prettyPrint() + "+";
+ }
+ eq DifferenceNewObject.prettyPrint() {
+ return "+DifferenceNewObject of " + getObject().prettyPrint() + "+";
}
- syn String PickAndPlace.toString() {
- return "+PickAndPlace by " + getRobotToExecute() + " of " + getObjectToPick() + " -> " + getTargetLocation() + "+";
+ 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() + "+";
}
}
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
index 6955828c67b6111c63c6d310571b6360a18d3ba2..dbb21249db70b8a3a05ded34b61b5d0cb94a06b6 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast
@@ -3,11 +3,13 @@ WorldModelB ::= Robot* [MyScene:Scene] [OtherScene:LogicalScene] /NextOperation:
Robot ::= <Name:String> CanReachObjectOfInterestWrapper ;
abstract Difference ;
+rel Difference.Object -> LogicalMovableObject ;
DifferenceObjectAtWrongPlace : Difference ;
-rel DifferenceObjectAtWrongPlace.Object -> LogicalMovableObject ;
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 03c3b07382239c72dc27368404abaecc017df30c..f41e113a0cc5102bd0b91b351aec4a54aa2fc119 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
@@ -7,7 +7,6 @@ import de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
-import java.io.File;
import java.nio.charset.StandardCharsets;
import java.nio.file.Path;
import java.nio.file.Paths;
@@ -23,7 +22,7 @@ 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 static final Logger logger = LogManager.getLogger(MainB.class);
private final static String TOPIC_MODEL = "place-b/model";
private final static String TOPIC_STATUS = "place-b/status";
@@ -35,7 +34,7 @@ public class MainB {
private final static String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A = "place-a/logical/update";
- private final static String TOPIC_DEMO_MOVE_objectRed1_RED = "place-a/demo/move/objectRed1/red";
+ private final static String TOPIC_DEMO_MOVE_objectRed1_RED = "place-b/demo/move/objectRed1/red";
private MqttHandler mainHandler;
private WorldModelB model;
@@ -60,13 +59,9 @@ public class MainB {
mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes)));
- mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes -> {
- ObjectOfInterest obj = model.getMyScene().resolveObjectOfInterest("objectRed1");
- if (obj != null) {
- // move objectRed1 to binRed
- obj.setPosition(Position.of(0.06f, 0.49f, 0.8325f));
- }
- });
+ mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes ->
+ UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed")
+ );
/// Reading scene and robots
de.tudresden.inf.st.ceti.Scene scene = readScene(
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..9e55144a3e662ba84e7eb7805cfdf0ac5facca01 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
@@ -3,6 +3,7 @@ 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;
@@ -12,6 +13,9 @@ import java.nio.charset.StandardCharsets;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
+import java.util.concurrent.TimeUnit;
+
+import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
/**
* Testing features for placeB.
@@ -30,7 +34,56 @@ public class SimpleMainB {
private void run(String[] args) throws Exception {
// testBuildModelB();
// testReceivingModelB();
- testReachability();
+// testReachability();
+ testWeirdBehaviour();
+ }
+
+ 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.connectOtherScene(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 {
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 d8fda0b01a1ccc92f57fabcc56ea6ecd9bcc80fa..b837ab3c1a4572d2a7a88ee249e73fc71049498a 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,6 +42,40 @@ 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 + "\"";
@@ -92,9 +121,9 @@ public class UtilB {
} 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();
}