diff --git a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle index 2281b3436d3e0b1ba6c88335e6f366642143ab07..79eb8724c34a42b7138faa8b58a9ecdf78a6cd04 100644 --- a/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle +++ b/buildSrc/src/main/groovy/ros3rag.java-ragconnect-conventions.gradle @@ -12,8 +12,8 @@ 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' - ragconnectClasspath group: 'de.tudresden.inf.st', name: 'ragconnect', version: '0.3.0' -// ragconnectClasspath fileTree(include: ['ragconnect-0.2.5.jar'], dir: '../libs') +// ragconnectClasspath group: 'de.tudresden.inf.st', name: 'ragconnect', version: '0.3.0' + ragconnectClasspath fileTree(include: ['ragconnect-0.3.1.jar'], dir: '../libs') // 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/libs/ragconnect-0.3.1.jar b/libs/ragconnect-0.3.1.jar new file mode 100644 index 0000000000000000000000000000000000000000..faabd7a763be13599c78fd851038b6ec69dbb79d Binary files /dev/null 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 edaf5b36248eb2c642e81ff299a83a54dfaf28f6..ed44d6ae3c01d9886b549ad1652ee2430b7a4043 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -2,32 +2,6 @@ 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 { syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) { for (DropOffLocation location : getDropOffLocationList()) { @@ -99,19 +73,19 @@ aspect Computation { 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)); } } } @@ -147,33 +121,67 @@ aspect Navigation { } aspect Printing { - syn String LogicalScene.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).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).append("\n"); } - sb.append("\n"); } return sb.toString(); } - 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 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 LogicalScene.prettyPrint() { + StringBuilder sb = new StringBuilder(); + if (getNumLogicalDropOffLocation() == 0) { + sb.append(" no locations\n"); + } else { + for (var location : getLogicalDropOffLocationList()) { + sb.append(" location ").append(location); + 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); + 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.toString() = "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">"; + syn String LogicalDropOffLocation.toString() = "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}"; } aspect ConvenienceMethods { 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..920de2346e4f68a7b904f91a5379cdad261e8415 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,14 +1,10 @@ package de.tudresden.inf.st.placeA; -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.placeA.ast.*; 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; @@ -32,6 +28,9 @@ public class MainA { 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 final static String TOPIC_DEMO_MOVE_objectRed1_BLUE = "place-a/demo/move/objectRed1/blue"; + private final static String TOPIC_DEMO_MOVE_objectRed1_RED = "place-a/demo/move/objectRed1/red"; + private MqttHandler mainHandler; private WorldModelA model; @@ -56,6 +55,13 @@ 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"); + }); + /// Reading scene and robot de.tudresden.inf.st.ceti.Scene scene = readScene( UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene) @@ -79,14 +85,33 @@ public class MainA { 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); + mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray()); +// logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT); Runtime.getRuntime().addShutdownHook(new Thread(this::close)); exitCondition.await(); } + private void 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."); @@ -94,9 +119,9 @@ public class MainA { model.setRobot(new Robot().setName(name)); } - private void logStatus(String prefix) { - logger.info(prefix); - String content = UtilA.getModelInfos(model); + private void logStatus(String message) { + logger.info(message); + String content = UtilA.getModelInfos(model, message.equals("Start") || message.startsWith("detail")); logger.info("WorldModelA\n{}", content); if (mainHandler != null) { mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8)); 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..25721619b0b5aad42ac6bc0fd57600df56e11ea2 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 @@ -25,11 +25,20 @@ public class UtilA { return new ExposingASTNode().exposed_apply_ConvertScene(scene); } - public static String getModelInfos(WorldModelA model) { + 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()); 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..03c3b07382239c72dc27368404abaecc017df30c 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 @@ -35,6 +35,8 @@ 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 MqttHandler mainHandler; private WorldModelB model; @@ -58,6 +60,14 @@ 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)); + } + }); + /// Reading scene and robots de.tudresden.inf.st.ceti.Scene scene = readScene( UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene) @@ -93,8 +103,8 @@ public class MainB { 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); + mainHandler.publish(TOPIC_MY_SCENE_INIT, scene.toByteArray()); +// logger.fatal("Skipping publishing to {} for now", TOPIC_MY_SCENE_INIT); Runtime.getRuntime().addShutdownHook(new Thread(this::close)); @@ -103,7 +113,7 @@ public class MainB { private void logStatus(String message) { logger.info(message); - String content = UtilB.getModelInfos(model); + String content = UtilB.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)); 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..d8fda0b01a1ccc92f57fabcc56ea6ecd9bcc80fa 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 @@ -47,12 +47,39 @@ public class UtilB { return result; } + 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());