Skip to content
Snippets Groups Projects
Commit 0597de38 authored by René Schöne's avatar René Schöne
Browse files

Working on strange problems.

- temporarily use unpublished, somehow fixed version of ragconnect
parent bcd76ebd
No related branches found
No related tags found
1 merge request!1Multiple scenes, multiple robots and more
Pipeline #9730 passed
...@@ -12,8 +12,8 @@ dependencies { ...@@ -12,8 +12,8 @@ dependencies {
api group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11' api group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11'
grammar2umlClasspath group: 'de.tudresden.inf.st', name: 'grammar2uml', version: '0.1.1' 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 group: 'de.tudresden.inf.st', name: 'ragconnect', version: '0.3.0'
// ragconnectClasspath fileTree(include: ['ragconnect-0.2.5.jar'], dir: '../libs') ragconnectClasspath fileTree(include: ['ragconnect-0.3.1.jar'], dir: '../libs')
// implementation group: 'de.tudresden.inf.st', name: 'dumpAstWithPlantuml', version: '0.3.5' // implementation group: 'de.tudresden.inf.st', name: 'dumpAstWithPlantuml', version: '0.3.5'
// implementation fileTree(include: ['dumpAstWithPlantuml-0.3.5.jar'], dir: '../libs') // implementation fileTree(include: ['dumpAstWithPlantuml-0.3.5.jar'], dir: '../libs')
......
File added
...@@ -2,32 +2,6 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation; ...@@ -2,32 +2,6 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import java.util.*; 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 { aspect Resolving {
syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) { syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) {
for (DropOffLocation location : getDropOffLocationList()) { for (DropOffLocation location : getDropOffLocationList()) {
...@@ -99,19 +73,19 @@ aspect Computation { ...@@ -99,19 +73,19 @@ aspect Computation {
syn LogicalScene Scene.getLogicalScene() { syn LogicalScene Scene.getLogicalScene() {
var result = new LogicalScene(); var result = new LogicalScene();
Map<MovableObject, LogicalMovableObject> objects = new HashMap<>(); 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()) { for (DropOffLocation location : getDropOffLocationList()) {
var logicalLocation = new LogicalDropOffLocation(); var logicalLocation = new LogicalDropOffLocation();
logicalLocation.setName(location.getName()); logicalLocation.setName(location.getName());
result.addLogicalDropOffLocation(logicalLocation); result.addLogicalDropOffLocation(logicalLocation);
for (MovableObject movableObject : getMovableObjectList()) { for (MovableObject movableObject : getMovableObjectList()) {
if (movableObject.isLocatedAt(location)) { if (movableObject.isLocatedAt(location)) {
LogicalMovableObject logicalMovableObject = objects.computeIfAbsent(movableObject, k -> { logicalLocation.addContainedObject(objects.get(movableObject));
var newLogicalMovableObject = new LogicalMovableObject();
newLogicalMovableObject.setName(k.getName());
result.addLogicalMovableObject(newLogicalMovableObject);
return newLogicalMovableObject;
});
logicalLocation.addContainedObject(logicalMovableObject);
} }
} }
} }
...@@ -147,8 +121,43 @@ aspect Navigation { ...@@ -147,8 +121,43 @@ aspect Navigation {
} }
aspect Printing { aspect Printing {
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();
if (getNumDropOffLocation() == 0) {
sb.append(" no locations\n");
} else {
for (var location : getDropOffLocationList()) {
sb.append(" location ").append(location).append("\n");
}
}
if (getNumMovableObject() == 0) {
sb.append(" no objects\n");
} else {
for (var obj : getMovableObjectList()) {
sb.append(" obj ").append(obj).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 LogicalScene.prettyPrint() { syn String LogicalScene.prettyPrint() {
StringBuilder sb = new StringBuilder(); StringBuilder sb = new StringBuilder();
if (getNumLogicalDropOffLocation() == 0) {
sb.append(" no locations\n");
} else {
for (var location : getLogicalDropOffLocationList()) { for (var location : getLogicalDropOffLocationList()) {
sb.append(" location ").append(location); sb.append(" location ").append(location);
if (!location.getContainedObjectList().isEmpty()) { if (!location.getContainedObjectList().isEmpty()) {
...@@ -156,6 +165,10 @@ aspect Printing { ...@@ -156,6 +165,10 @@ aspect Printing {
} }
sb.append("\n"); sb.append("\n");
} }
}
if (getNumLogicalMovableObject() == 0) {
sb.append(" no objects\n");
} else {
for (var obj : getLogicalMovableObjectList()) { for (var obj : getLogicalMovableObjectList()) {
sb.append(" obj ").append(obj); sb.append(" obj ").append(obj);
if (obj.hasLocatedAt()) { if (obj.hasLocatedAt()) {
...@@ -163,17 +176,12 @@ aspect Printing { ...@@ -163,17 +176,12 @@ aspect Printing {
} }
sb.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 sb.toString();
return java.util.stream.StreamSupport.stream(Spliterators.spliteratorUnknownSize(this.iterator(), 16), false).map(toString::apply).collect(java.util.stream.Collectors.joining(", ", "[", "]"));
} }
syn String LogicalMovableObject.toString() = "<obj " + getName() + "@" + hashCode() + ">"; syn String LogicalMovableObject.toString() = "<obj " + getName() + "@" + Integer.toHexString(hashCode()) + ">";
syn String LogicalDropOffLocation.toString() = "{loc " + getName() + "@" + hashCode() + "}"; syn String LogicalDropOffLocation.toString() = "{loc " + getName() + "@" + Integer.toHexString(hashCode()) + "}";
} }
aspect ConvenienceMethods { aspect ConvenienceMethods {
......
package de.tudresden.inf.st.placeA; package de.tudresden.inf.st.placeA;
import de.tudresden.inf.st.placeA.ast.MqttHandler; import de.tudresden.inf.st.placeA.ast.*;
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 de.tudresden.inf.st.ros3rag.common.Util;
import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger; import org.apache.logging.log4j.Logger;
import java.io.File;
import java.nio.charset.StandardCharsets; import java.nio.charset.StandardCharsets;
import java.util.concurrent.CountDownLatch; import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
...@@ -32,6 +28,9 @@ public class MainA { ...@@ -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_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_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 MqttHandler mainHandler;
private WorldModelA model; private WorldModelA model;
...@@ -56,6 +55,13 @@ public class MainA { ...@@ -56,6 +55,13 @@ public class MainA {
mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown()); mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes))); 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 /// Reading scene and robot
de.tudresden.inf.st.ceti.Scene scene = readScene( de.tudresden.inf.st.ceti.Scene scene = readScene(
UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene) UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene)
...@@ -79,14 +85,33 @@ public class MainA { ...@@ -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 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); 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()); mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray());
logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT); // logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT);
Runtime.getRuntime().addShutdownHook(new Thread(this::close)); Runtime.getRuntime().addShutdownHook(new Thread(this::close));
exitCondition.await(); 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) { private void setRobot(String name) {
if (model.getRobot() != null) { if (model.getRobot() != null) {
logger.error("Robot already set. Overriding with newly found name."); logger.error("Robot already set. Overriding with newly found name.");
...@@ -94,9 +119,9 @@ public class MainA { ...@@ -94,9 +119,9 @@ public class MainA {
model.setRobot(new Robot().setName(name)); model.setRobot(new Robot().setName(name));
} }
private void logStatus(String prefix) { private void logStatus(String message) {
logger.info(prefix); logger.info(message);
String content = UtilA.getModelInfos(model); String content = UtilA.getModelInfos(model, message.equals("Start") || message.startsWith("detail"));
logger.info("WorldModelA\n{}", content); logger.info("WorldModelA\n{}", content);
if (mainHandler != null) { if (mainHandler != null) {
mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8)); mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8));
......
...@@ -25,11 +25,20 @@ public class UtilA { ...@@ -25,11 +25,20 @@ public class UtilA {
return new ExposingASTNode().exposed_apply_ConvertScene(scene); 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(); StringBuilder sb = new StringBuilder();
sb.append("myRobot: ") sb.append("myRobot: ")
.append(model.getRobot().getName()) .append(model.getRobot().getName())
.append("\n"); .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:"); sb.append("myLogicalScene:");
if (model.hasScene()) { if (model.hasScene()) {
sb.append("\n").append(model.getScene().getLogicalScene().prettyPrint()); sb.append("\n").append(model.getScene().getLogicalScene().prettyPrint());
......
...@@ -35,6 +35,8 @@ public class MainB { ...@@ -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_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 MqttHandler mainHandler;
private WorldModelB model; private WorldModelB model;
...@@ -58,6 +60,14 @@ public class MainB { ...@@ -58,6 +60,14 @@ public class MainB {
mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown()); mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown());
mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes))); 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 /// Reading scene and robots
de.tudresden.inf.st.ceti.Scene scene = readScene( de.tudresden.inf.st.ceti.Scene scene = readScene(
UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene) UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene)
...@@ -93,8 +103,8 @@ public class MainB { ...@@ -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 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); 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()); mainHandler.publish(TOPIC_MY_SCENE_INIT, scene.toByteArray());
logger.fatal("Skipping publishing to {} for now", TOPIC_MY_SCENE_INIT); // logger.fatal("Skipping publishing to {} for now", TOPIC_MY_SCENE_INIT);
Runtime.getRuntime().addShutdownHook(new Thread(this::close)); Runtime.getRuntime().addShutdownHook(new Thread(this::close));
...@@ -103,7 +113,7 @@ public class MainB { ...@@ -103,7 +113,7 @@ public class MainB {
private void logStatus(String message) { private void logStatus(String message) {
logger.info(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); logger.info("WorldModelB\n{}", content);
if (mainHandler != null) { if (mainHandler != null) {
mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8)); mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8));
......
...@@ -47,12 +47,39 @@ public class UtilB { ...@@ -47,12 +47,39 @@ public class UtilB {
return result; 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) { static String getModelInfos(WorldModelB model) {
return getModelInfos(model, false);
}
static String getModelInfos(WorldModelB model, boolean detailed) {
StringBuilder sb = new StringBuilder(); StringBuilder sb = new StringBuilder();
sb.append("myRobots: ") sb.append("myRobots: ")
.append(model.getRobotList().prettyPrint( .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"); .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:"); sb.append("myLogicalScene:");
if (model.hasMyScene()) { if (model.hasMyScene()) {
sb.append("\n").append(model.getMyScene().getLogicalScene().prettyPrint()); sb.append("\n").append(model.getMyScene().getLogicalScene().prettyPrint());
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment