diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle index 4fa92cd8167fd9f2c217afc3ccba265f9786117e..d273f65ea3c43b0cd6b0ce2f0b13589098fef608 100644 --- a/ros3rag.placeB/build.gradle +++ b/ros3rag.placeB/build.gradle @@ -21,6 +21,7 @@ task simpleRun(type: JavaExec) { dependencies { implementation project(':ros3rag.common') + testImplementation group: 'org.junit.jupiter', name: 'junit-jupiter-params', version: "${jupiter_version}" } ext.sharedJastAddDir = 'src/main/jastadd/shared' diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag index c0613d8e968ed6f9c79cd1d29bba32d0377bd4e5..7dd4ee10fbe6bad66d3a5dc7542af09e486a374e 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag @@ -4,23 +4,30 @@ aspect RobotReachabilityToBFS { if (!hasMyScene()) { return result; } - Map<LogicalDropOffLocation, Vertex> mapping = new HashMap<>(); + Map<LogicalObjectOfInterest, Vertex> mapping = new HashMap<>(); for (LogicalDropOffLocation loc : getMyScene().getLogicalScene().getLogicalDropOffLocationList()) { Vertex vertex = new Vertex(); - vertex.setLocation(loc); + vertex.setObjectOfInterest(loc); result.addVertex(vertex); mapping.put(loc, vertex); } + for (LogicalMovableObject obj : getMyScene().getLogicalScene().getLogicalMovableObjectList()) { + Vertex vertex = new Vertex(); + vertex.setObjectOfInterest(obj); + result.addVertex(vertex); + mapping.put(obj, vertex); + } for (Robot robot : getRobotList()) { List<LogicalObjectOfInterest> reachableLocations = robot.reachableObjects().stream() .filter(LogicalObjectOfInterest::isLogicalDropOffLocation) .collect(java.util.stream.Collectors.toList()); - for (LogicalObjectOfInterest obj : reachableLocations) { + // bidirectional edges between locations, unidirectional edges from all reachable movableobjects to locations + for (LogicalObjectOfInterest obj : robot.reachableObjects()) { for (LogicalObjectOfInterest other : reachableLocations) { if (obj == other) { continue; } Edge edge = new Edge(); edge.setRobot(robot); - edge.setBidirectional(true); + edge.setBidirectional(obj.isLogicalDropOffLocation()); edge.setFrom(mapping.get(obj)); edge.setTo(mapping.get(other)); result.addEdge(edge); @@ -30,12 +37,12 @@ aspect RobotReachabilityToBFS { return result; } - syn Optional<Vertex> LogicalDropOffLocation.correspondingVertex() { + syn Optional<Vertex> LogicalObjectOfInterest.correspondingVertex() { if (!worldModelB().hasMyScene()) { return Optional.empty(); } for (Vertex v : worldModelB().toReachabilityGraph().getVertexList()) { - if (v.getLocation() == this) { + if (v.getObjectOfInterest() == this) { return Optional.of(v); } } @@ -45,7 +52,7 @@ aspect RobotReachabilityToBFS { aspect Printint { public String Vertex.toString() { - return "V(" + getLocation().getName() + ")"; + return "V(" + getObjectOfInterest().getName() + ")"; } public String Edge.toString() { return "E[" + getRobot().getName() + "]"; diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast index 914e7b4ff9f5ee02cea3de883edb390705de083e..62d0b4071475bea2960b6aed0becbad54aa9eaaf 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.relast @@ -1,2 +1,2 @@ -rel Vertex.Location -> LogicalDropOffLocation ; +rel Vertex.ObjectOfInterest -> LogicalObjectOfInterest ; rel Edge.Robot -> Robot ; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 14ada75fd4a270301018dfdef95eafff3e203bcb..3c43e9ba77dfae3daf1622b93cf93abac913a651 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -11,18 +11,18 @@ aspect Computation { var myLogicalScene = getMyScene().getLogicalScene(); for (LogicalMovableObject otherObject : getOtherScene().getLogicalMovableObjectList()) { LogicalObjectOfInterest myGenericObject = myLogicalScene.resolveLogicalObjectOfInterest(otherObject.getName()); - if (!myGenericObject.isLogicalMovableObject()) { - System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!"); - continue; - } - LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject(); - if (myObject == null) { + if (myGenericObject == null) { // new object, currently not handled var diff = new DifferenceNewObject(); diff.setObject(otherObject); result.add(diff); continue; } + if (!myGenericObject.isLogicalMovableObject()) { + System.err.println("MovableObject " + otherObject.getName() + " is a location in myScene. Skipping!"); + continue; + } + LogicalMovableObject myObject = myGenericObject.asLogicalMovableObject(); if (myObject.hasLocatedAt() && !otherObject.hasLocatedAt()) { var diff = new DifferenceObjectMisplaced(); diff.setObject(myObject); @@ -62,7 +62,7 @@ aspect Computation { //--- computeOperations --- syn List<Operation> Difference.computeOperations(); eq DifferenceObjectAtWrongPlace.computeOperations() { - return getPreviousLocation().correspondingVertex().map(previousVertex -> { + return (hasPreviousLocation() ? getPreviousLocation() : getObject()).correspondingVertex().map(previousVertex -> { return getNewLocation().correspondingVertex().map(newVertex -> { List<Edge> shortestPath = previousVertex.BFS(newVertex); if (shortestPath == null || shortestPath.isEmpty()) { @@ -72,7 +72,7 @@ aspect Computation { Vertex transit = previousVertex; for (Edge edge : shortestPath) { Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom(); - result.add(createPickAndPlace(edge.getRobot(), getObject(), target.getLocation())); + result.add(createPickAndPlace(edge.getRobot(), getObject(), target.getObjectOfInterest().asLogicalDropOffLocation())); transit = target; } return result; diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java index da21587ed97196603a447fe09dd3e4789a3b2663..26681503def3124be05fe7d15a25c61451c3426a 100644 --- a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java +++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java @@ -1,10 +1,13 @@ package de.tudresden.inf.st.placeB; import de.tudresden.inf.st.placeB.ast.*; -import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.ValueSource; import java.util.List; +import static de.tudresden.inf.st.placeB.TestUtils.*; +import static de.tudresden.inf.st.placeB.TestUtils.addMyObjects; import static org.assertj.core.api.Assertions.assertThat; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -15,44 +18,49 @@ import static org.junit.jupiter.api.Assertions.assertTrue; * @author rschoene - Initial contribution */ public class TestComputeOperations { - @Test - public void testNoMovePossible() { - WorldModelB model = TestUtils.newModel(); - TestUtils.addObjects(model, "x"); - TestUtils.addLocations(model, "red", "blue", "green"); - TestUtils.addRobots(model, "arm1", "arm2"); - TestUtils.addReachability(model, "arm1", "red", "blue"); - TestUtils.addReachability(model, "arm2", "blue"); + + @ParameterizedTest(name = "testNoMovePossible [emptyPreviousLocation = {argumentsWithNames}]") + @ValueSource(booleans = {true, false}) + public void testNoMovePossible(boolean emptyPreviousLocation) { + WorldModelB model = newModel(); + addMyObjects(model, "x"); + addMyLocations(model, "red", "blue", "green"); + addRobots(model, "arm1", "arm2"); + addReachability(model, "arm1", "red", "blue", "x"); + addReachability(model, "arm2", "blue"); LogicalScene logicalScene = model.getMyScene().getLogicalScene(); LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation(); - DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green); + DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, green); List<Operation> operations = diff.computeOperations(); assertThat(operations).isNotEmpty(); assertThat(operations).hasSize(1); Operation op = operations.get(0); assertTrue(op.isErrorOperation()); - assertEquals("+Error: No sequence of operations to move x from red to green+", op.prettyPrint()); + assertEquals(emptyPreviousLocation ? + "+Error: No sequence of operations to move x to green+" : + "+Error: No sequence of operations to move x from red to green+", op.prettyPrint()); } - @Test - public void testDirectMove() { - WorldModelB model = TestUtils.newModel(); - TestUtils.addObjects(model, "x"); - TestUtils.addLocations(model, "red", "blue", "green"); - TestUtils.addRobots(model, "arm1", "arm2"); - TestUtils.addReachability(model, "arm1", "red", "blue"); - TestUtils.addReachability(model, "arm2", "blue"); + @ParameterizedTest(name = "testDirectMove [emptyPreviousLocation = {argumentsWithNames}]") + @ValueSource(booleans = {true, false}) + public void testDirectMove(boolean emptyPreviousLocation) { + WorldModelB model = newModel(); + addMyObjects(model, "x"); + addMyLocations(model, "red", "blue", "green"); + addRobots(model, "arm1", "arm2"); + addReachability(model, "arm1", "red", "blue", "x"); + addReachability(model, "arm2", "blue"); LogicalScene logicalScene = model.getMyScene().getLogicalScene(); LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation(); Robot arm1 = model.findRobot("arm1").orElseThrow(); - DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, blue); + DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, blue); List<Operation> operations = diff.computeOperations(); assertThat(operations).isNotEmpty(); @@ -65,14 +73,15 @@ public class TestComputeOperations { assertEquals(blue, pickAndPlace.getTargetLocation()); } - @Test - public void testIndirectMoveWith2Robots() { - WorldModelB model = TestUtils.newModel(); - TestUtils.addObjects(model, "x"); - TestUtils.addLocations(model, "red", "blue", "green"); - TestUtils.addRobots(model, "arm1", "arm2"); - TestUtils.addReachability(model, "arm1", "red", "blue"); - TestUtils.addReachability(model, "arm2", "blue", "green"); + @ParameterizedTest(name = "testIndirectMoveWith2Robots [emptyPreviousLocation = {argumentsWithNames}]") + @ValueSource(booleans = {true, false}) + public void testIndirectMoveWith2Robots(boolean emptyPreviousLocation) { + WorldModelB model = newModel(); + addMyObjects(model, "x"); + addMyLocations(model, "red", "blue", "green"); + addRobots(model, "arm1", "arm2"); + addReachability(model, "arm1", "red", "blue", "x"); + addReachability(model, "arm2", "blue", "green"); LogicalScene logicalScene = model.getMyScene().getLogicalScene(); LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); @@ -81,7 +90,7 @@ public class TestComputeOperations { LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation(); Robot arm1 = model.findRobot("arm1").orElseThrow(); Robot arm2 = model.findRobot("arm2").orElseThrow(); - DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green); + DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, green); List<Operation> operations = diff.computeOperations(); assertThat(operations).isNotEmpty(); @@ -101,22 +110,23 @@ public class TestComputeOperations { assertEquals(green, pickAndPlace2.getTargetLocation()); } - @Test - public void testIndirectMoveWith3Robots() { + @ParameterizedTest(name = "testIndirectMoveWith3Robots [emptyPreviousLocation = {argumentsWithNames}]") + @ValueSource(booleans = {true, false}) + public void testIndirectMoveWith3Robots(boolean emptyPreviousLocation) { /* * red -(arm1)-> blue --(arm3)-, * `-(arm2)-> green <-------' * `--(arm4)-> yellow -(arm5)-> purple */ - WorldModelB model = TestUtils.newModel(); - TestUtils.addObjects(model, "x"); - TestUtils.addLocations(model, "red", "blue", "green", "yellow", "purple"); - TestUtils.addRobots(model, "arm1", "arm2", "arm3", "arm4", "arm5"); - TestUtils.addReachability(model, "arm1", "red", "blue"); - TestUtils.addReachability(model, "arm2", "red", "green"); - TestUtils.addReachability(model, "arm3", "blue", "green"); - TestUtils.addReachability(model, "arm4", "green", "yellow"); - TestUtils.addReachability(model, "arm5", "yellow", "purple"); + WorldModelB model = newModel(); + addMyObjects(model, "x"); + addMyLocations(model, "red", "blue", "green", "yellow", "purple"); + addRobots(model, "arm1", "arm2", "arm3", "arm4", "arm5"); + addReachability(model, "arm1", "red", "blue", "x"); + addReachability(model, "arm2", "red", "green", "x"); + addReachability(model, "arm3", "blue", "green"); + addReachability(model, "arm4", "green", "yellow"); + addReachability(model, "arm5", "yellow", "purple"); LogicalScene logicalScene = model.getMyScene().getLogicalScene(); LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); @@ -127,7 +137,7 @@ public class TestComputeOperations { Robot arm2 = model.findRobot("arm2").orElseThrow(); Robot arm4 = model.findRobot("arm4").orElseThrow(); Robot arm5 = model.findRobot("arm5").orElseThrow(); - DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, purple); + DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, emptyPreviousLocation ? null : red, purple); List<Operation> operations = diff.computeOperations(); assertThat(operations).isNotEmpty(); diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java new file mode 100644 index 0000000000000000000000000000000000000000..5c98b039d1c8d973a8e89297cfb75ad2c3e78139 --- /dev/null +++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestDifference.java @@ -0,0 +1,202 @@ +package de.tudresden.inf.st.placeB; + +import de.tudresden.inf.st.placeB.ast.*; +import org.junit.jupiter.api.Test; + +import static de.tudresden.inf.st.placeB.TestUtils.*; +import static org.assertj.core.api.Assertions.assertThat; +import static org.assertj.core.api.Assertions.tuple; +import static org.junit.jupiter.api.Assertions.*; + +/** + * Testing computing differences. + * + * @author rschoene - Initial contribution + */ +public class TestDifference { + + @Test + public void testDifferenceObjectAtWrongPlaceNoPreviousLocation() { + // myScene: object (x) not contained in any location + // targetScene: object (x) in location (red) + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(4, 4, 4), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + addContainedObjects(model, "red", "x"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + assertFalse(x.hasLocatedAt()); + + LogicalDropOffLocation otherRed = model.getOtherScene().resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(1, diffs.getNumChild()); + Difference diff = diffs.getChild(0); + assertThat(diff).isInstanceOf(DifferenceObjectAtWrongPlace.class); + DifferenceObjectAtWrongPlace wrongPlace = (DifferenceObjectAtWrongPlace) diff; + assertEquals(x, wrongPlace.getObject()); + assertNull(wrongPlace.getPreviousLocation()); + assertEquals(otherRed, wrongPlace.getNewLocation()); + } + + @Test + public void testDifferenceObjectAtWrongPlaceOtherPreviousLocation() { + // myScene: object (x) contained in another location (blue) + // targetScene: object (x) in location (red) + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "blue", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + addContainedObjects(model, "red", "x"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + LogicalDropOffLocation blue = myLogicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation(); + assertEquals(blue, x.getLocatedAt()); + LogicalDropOffLocation otherRed = model.getOtherScene().resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(1, diffs.getNumChild()); + Difference diff = diffs.getChild(0); + assertThat(diff).isInstanceOf(DifferenceObjectAtWrongPlace.class); + DifferenceObjectAtWrongPlace wrongPlace = (DifferenceObjectAtWrongPlace) diff; + assertEquals(x, wrongPlace.getObject()); + assertEquals(blue, wrongPlace.getPreviousLocation()); + assertEquals(otherRed, wrongPlace.getNewLocation()); + } + + @Test + public void testDifferenceObjectMisplaced() { + // myScene: object (x) contained in a location (red) + // targetScene: object (x) not contained in any location + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + LogicalDropOffLocation red = myLogicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); + assertEquals(red, x.getLocatedAt()); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(1, diffs.getNumChild()); + Difference diff = diffs.getChild(0); + assertThat(diff).isInstanceOf(DifferenceObjectMisplaced.class); + DifferenceObjectMisplaced misplaced = (DifferenceObjectMisplaced) diff; + assertEquals(x, misplaced.getObject()); + assertEquals(red, misplaced.getPreviousLocation()); + } + + @Test + public void testNoDifferenceNoContainment() { + // myScene: object (x) not contained in any location + // targetScene: object (x) not contained in any location + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(8, 8, 8), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + assertFalse(x.hasLocatedAt()); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(0, diffs.getNumChild()); + } + + @Test + public void testNoDifferenceSameLocation() { + // myScene: object (x) contained in same location (red) + // targetScene: object (x) contained in a location (red) + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + addContainedObjects(model, "red", "x"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + LogicalDropOffLocation red = myLogicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); + assertEquals(red, x.getLocatedAt()); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(0, diffs.getNumChild()); + } + + @Test + public void testDifferenceNewObject() { + // myScene: object (x) not known + // targetScene: object (x) exists, not contained in any location + WorldModelB model = newModel(); + addMyLocation(model, "red", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x"); + addOtherLocations(model, "red"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + assertNull(myLogicalScene.resolveLogicalObjectOfInterest("x")); + + LogicalMovableObject otherX = model.getOtherScene().resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(1, diffs.getNumChild()); + Difference diff = diffs.getChild(0); + assertThat(diff).isInstanceOf(DifferenceNewObject.class); + DifferenceNewObject diffNewObject = (DifferenceNewObject) diff; + assertEquals(otherX, diffNewObject.getObject()); + } + + @Test + public void testMultipleDifferenceObjectAtWrongPlace() { + // myScene: objects (x, y) contained in other locations (blue, green) + // targetScene: objects (x, y) in location (red) + WorldModelB model = newModel(); + addMyObject(model, "x", new Position(0.5f, 0.5f, 0.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyObject(model, "y", new Position(2.5f, 2.5f, 2.5f), noRotation(), new Size(0.5f, 0.5f, 0.5f)); + addMyLocation(model, "blue", new Position(0, 0, 0), noRotation(), new Size(1, 1, 1)); + addMyLocation(model, "green", new Position(2, 2, 2), noRotation(), new Size(1, 1, 1)); + + addOtherObjects(model, "x", "y"); + addOtherLocations(model, "red"); + addContainedObjects(model, "red", "x", "y"); + + LogicalScene myLogicalScene = model.getMyScene().getLogicalScene(); + LogicalMovableObject x = myLogicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject(); + LogicalMovableObject y = myLogicalScene.resolveLogicalObjectOfInterest("y").asLogicalMovableObject(); + LogicalDropOffLocation blue = myLogicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation(); + LogicalDropOffLocation green = myLogicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation(); + assertEquals(blue, x.getLocatedAt()); + assertEquals(green, y.getLocatedAt()); + + LogicalDropOffLocation otherRed = model.getOtherScene().resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation(); + + + JastAddList<Difference> diffs = model.diffScenes(); + assertNotNull(diffs); + assertEquals(2, diffs.getNumChild()); + assertThat(diffs).allMatch(d -> d instanceof DifferenceObjectAtWrongPlace); + assertThat(diffs).map(diff -> (DifferenceObjectAtWrongPlace) diff) + .extracting("Object", "PreviousLocation", "NewLocation") + .containsOnly(tuple(x, blue, otherRed), + tuple(y, green, otherRed)); + } +} diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java index befdb392a0df2ff1cd4f19288d53b0306a2aa971..699e3e32c54c18234ef562fb4dc526e2f70b3474 100644 --- a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java +++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java @@ -12,33 +12,47 @@ public class TestUtils { WorldModelB model = new WorldModelB(); Scene scene = new Scene(); model.setMyScene(scene); + LogicalScene otherScene = new LogicalScene(); + model.setOtherScene(otherScene); return model; } - public static void addObjects(WorldModelB model, String... names) { + public static Orientation noRotation() { + return new Orientation().setW(1); + } + + public static void addMyObjects(WorldModelB model, String... names) { for (String name : names) { int index = model.getMyScene().getNumMovableObject(); - MovableObject obj = new MovableObject() - .setName(name) - .setPosition(new Position(-index, -index, -index)) - .setOrientation(new Orientation()) - .setSize(new Size(1, 1, 1)); - model.getMyScene().addMovableObject(obj); + addMyObject(model, name, new Position(-index, -index, -index), new Orientation(), new Size(1, 1, 1)); } } - public static void addLocations(WorldModelB model, String... names) { + public static void addMyObject(WorldModelB model, String name, Position position, Orientation orientation, Size size) { + MovableObject obj = new MovableObject() + .setName(name) + .setPosition(position) + .setOrientation(orientation) + .setSize(size); + model.getMyScene().addMovableObject(obj); + } + + public static void addMyLocations(WorldModelB model, String... names) { for (String name : names) { int index = model.getMyScene().getNumDropOffLocation(); - DropOffLocation obj = new DropOffLocation() - .setName(name) - .setPosition(new Position(+index, +index, +index)) - .setOrientation(new Orientation()) - .setSize(new Size(1, 1, 1)); - model.getMyScene().addDropOffLocation(obj); + addMyLocation(model, name, new Position(+index, +index, +index), new Orientation(), new Size(1, 1, 1)); } } + public static void addMyLocation(WorldModelB model, String name, Position position, Orientation orientation, Size size) { + DropOffLocation obj = new DropOffLocation() + .setName(name) + .setPosition(position) + .setOrientation(orientation) + .setSize(size); + model.getMyScene().addDropOffLocation(obj); + } + public static void addRobots(WorldModelB model, String... names) { for (String name : names) { Robot obj = new Robot().setName(name); @@ -55,4 +69,23 @@ public class TestUtils { robot.setCanReachObjectOfInterestWrapper(wrapper); } + public static void addOtherObjects(WorldModelB model, String... names) { + for (String name : names) { + model.getOtherScene().addLogicalMovableObject(new LogicalMovableObject().setName(name)); + } + } + + public static void addOtherLocations(WorldModelB model, String... names) { + for (String name : names) { + model.getOtherScene().addLogicalDropOffLocation(new LogicalDropOffLocation().setName(name)); + } + } + + public static void addContainedObjects(WorldModelB model, String locationName, String... objectNames) { + LogicalDropOffLocation location = model.getOtherScene().resolveLogicalObjectOfInterest(locationName).asLogicalDropOffLocation(); + for (String objectName : objectNames) { + location.addContainedObject(model.getOtherScene().resolveLogicalObjectOfInterest(objectName).asLogicalMovableObject()); + } + } + }