diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd b/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd index ea860569937ed09e8b5f576511c45e5ab565945d..b9dd1eebab6bf09ba350bce028e8f6bac4349695 100644 --- a/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd +++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.jadd @@ -1,4 +1,5 @@ aspect Resolving { + //--- resolveObjectOfInterest --- inh ObjectOfInterest Robot.resolveObjectOfInterest(String name); eq WorldModelA.getRobot().resolveObjectOfInterest(String name) { return getScene().resolveObjectOfInterest(name); @@ -7,5 +8,7 @@ aspect Resolving { 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 4ddddbdf13b7ee582048c4e1253171bc1d97b74a..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] /LogicalScene/ ; //Task* ; +WorldModelA ::= Robot [Scene] /LogicalScene/ ; Robot ::= <Name:String> ; - -//Task ::= ObjectToMove:MovableObject TargetLocation:DropOffLocation ; 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 9e55144a3e662ba84e7eb7805cfdf0ac5facca01..5b9c8e0cd38e208ca303cd35054bfb1aaf32820e 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 @@ -16,6 +16,7 @@ import java.nio.file.Paths; 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; /** * Testing features for placeB. @@ -35,7 +36,41 @@ public class SimpleMainB { // testBuildModelB(); // testReceivingModelB(); // testReachability(); - testWeirdBehaviour(); +// testWeirdBehaviour(); + printReachability(); + } + + private void printReachability() 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 { diff --git a/ros3rag.placeB/src/main/resources/config-scene-b.json b/ros3rag.placeB/src/main/resources/config-scene-b.json index 1cb23282e9fd86efd877f6c5e8d35ebd2ec04e47..ce9ba28dfa0cf2e66a8c31491492a75656fe6ca8 100644 --- a/ros3rag.placeB/src/main/resources/config-scene-b.json +++ b/ros3rag.placeB/src/main/resources/config-scene-b.json @@ -4,7 +4,7 @@ { "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": "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": "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": "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 } },