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

update scene-b config for reachability

parent 16932175
Branches
Tags
1 merge request!1Multiple scenes, multiple robots and more
Pipeline #9798 passed
aspect Resolving { aspect Resolving {
//--- resolveObjectOfInterest ---
inh ObjectOfInterest Robot.resolveObjectOfInterest(String name); inh ObjectOfInterest Robot.resolveObjectOfInterest(String name);
eq WorldModelA.getRobot().resolveObjectOfInterest(String name) { eq WorldModelA.getRobot().resolveObjectOfInterest(String name) {
return getScene().resolveObjectOfInterest(name); return getScene().resolveObjectOfInterest(name);
...@@ -7,5 +8,7 @@ aspect Resolving { ...@@ -7,5 +8,7 @@ aspect Resolving {
aspect Glue { aspect Glue {
class WorldModelA implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {} class WorldModelA implements de.tudresden.inf.st.ros3rag.common.SharedMainParts.WorldModelWrapper {}
//--- getLogicalScene ---
syn LogicalScene WorldModelA.getLogicalScene() = hasScene() ? getScene().getLogicalScene() : new LogicalScene(); syn LogicalScene WorldModelA.getLogicalScene() = hasScene() ? getScene().getLogicalScene() : new LogicalScene();
} }
WorldModelA ::= Robot [Scene] /LogicalScene/ ; //Task* ; WorldModelA ::= Robot [Scene] /LogicalScene/ ;
Robot ::= <Name:String> ; Robot ::= <Name:String> ;
//Task ::= ObjectToMove:MovableObject TargetLocation:DropOffLocation ;
...@@ -16,6 +16,7 @@ import java.nio.file.Paths; ...@@ -16,6 +16,7 @@ import java.nio.file.Paths;
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri; import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri;
import static de.tudresden.inf.st.ros3rag.common.Util.readScene;
/** /**
* Testing features for placeB. * Testing features for placeB.
...@@ -35,7 +36,41 @@ public class SimpleMainB { ...@@ -35,7 +36,41 @@ public class SimpleMainB {
// testBuildModelB(); // testBuildModelB();
// testReceivingModelB(); // testReceivingModelB();
// testReachability(); // 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 { private void testWeirdBehaviour() throws IOException {
......
...@@ -4,7 +4,7 @@ ...@@ -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": "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": "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": "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": "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": "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 } }, { "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 } },
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment