diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd index 7191036a12fb81d62adf271190ca419c935fe079..0d9fe88eb118fec13f1aadec7f72440fbefab760 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -3,6 +3,7 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import java.util.*; aspect Resolving { + //--- resolveObjectOfInterest --- syn ObjectOfInterest Scene.resolveObjectOfInterest(String name) { for (DropOffLocation location : getDropOffLocationList()) { if (location.getName().equals(name)) { @@ -16,6 +17,8 @@ aspect Resolving { } return null; } + + //--- resolveLogicalObjectOfInterest --- syn LogicalObjectOfInterest LogicalScene.resolveLogicalObjectOfInterest(String name) { for (LogicalDropOffLocation location : getLogicalDropOffLocationList()) { if (location.getName().equals(name)) { @@ -32,6 +35,7 @@ aspect Resolving { } aspect Computation { + //--- isLocatedAt --- syn boolean MovableObject.isLocatedAt(DropOffLocation location) { Orientation orient = location.getOrientation(); Position locationPosition = location.getPosition(); @@ -70,6 +74,7 @@ aspect Computation { LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight); } + //--- getLogicalScene --- syn LogicalScene Scene.getLogicalScene() { var result = new LogicalScene(); Map<MovableObject, LogicalMovableObject> objects = new HashMap<>(); @@ -94,6 +99,7 @@ aspect Computation { private static final double MovableObject.DELTA = 0.0001; + //--- LT --- /** * @return d1 <= d2 (within a DELTA) */ @@ -121,6 +127,7 @@ aspect Navigation { } aspect Printing { + //--- prettyPrint --- syn String JastAddList.prettyPrint() { return prettyPrint(Object::toString); } @@ -203,6 +210,7 @@ aspect Printing { } aspect ConvenienceMethods { + // --- of --- public static DropOffLocation DropOffLocation.of(String name, Position position, Orientation orientation, Size size) { var location = new DropOffLocation(); diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index bc1313f47df980940469d83e40dea8844a6bdbfe..54fc3553bb998d6f0cfd1d5f555b9f38b45c45bb 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -1,5 +1,7 @@ aspect Computation { syn nta LogicalDropOffLocation WorldModelB.unspecifiedLocation() = new LogicalDropOffLocation().setName("<unspecified>"); + + //--- diffScenes --- syn nta JastAddList<Difference> WorldModelB.diffScenes() { var result = new JastAddList<Difference>(); if (!hasMyScene() || !hasOtherScene()) { @@ -46,6 +48,7 @@ aspect Computation { return result; } + //--- diffToOperations --- syn nta JastAddList<Operation> WorldModelB.diffToOperations() { var result = new JastAddList<Operation>(); for (Difference difference : diffScenes()) { @@ -56,6 +59,7 @@ aspect Computation { return result; } + //--- computeOperations --- syn List<Operation> Difference.computeOperations(); eq DifferenceObjectAtWrongPlace.computeOperations() { // first, find robots that can reach the locations @@ -121,6 +125,7 @@ aspect Computation { return Collections.emptyList(); } + //--- createPickAndPlace --- private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) { var result = new PickAndPlace(); result.setRobotToExecute(robot); @@ -129,12 +134,12 @@ aspect Computation { return result; } + //--- getNextOperation --- syn Operation WorldModelB.getNextOperation() { return diffToOperations().getNumChild() > 0 ? diffToOperations().getChild(0) : new ErrorOperation("No operation computed!"); } -// syn List<String> locationIds - + //--- canReach --- syn boolean Robot.canReach(String objectName) { for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) { if (canReachObj.getObjectName().equals(objectName)) { @@ -156,6 +161,7 @@ aspect Computation { //} aspect AttributeMappings { + //--- toMergedSelection --- syn de.tudresden.inf.st.ceti.MergedSelection Operation.toMergedSelection(); eq ErrorOperation.toMergedSelection() { System.err.println(getErrorMessage()); @@ -168,17 +174,22 @@ 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); @@ -187,6 +198,7 @@ aspect GlueForShared { } aspect Printing { + //--- prettyPrint --- syn String Robot.prettyPrint() = "~Robot " + getName() + "~"; syn String Difference.prettyPrint(); @@ -212,6 +224,7 @@ aspect Printing { } aspect Resolving { + //--- findRobot --- public Optional<Robot> WorldModelB.findRobot(String name) { for (Robot robot : getRobotList()) { if (robot.getName().equals(name)) {