diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java index ee6c6f5a06806488aeb4ea5429f430217a3ff15f..a2e41ec5fb19c58cb05f2ff7eee7fa6e968ca41e 100644 --- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java +++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java @@ -22,7 +22,7 @@ public class Configuration { } public static class ConfigurationForB { - public List<String> reachability; + public String filenameReachability; public List<String> topicsSceneUpdate; public String topicCommand; } diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java index 0d57d35d53628f881c20562b7d57077108cfe6e0..14ea9b574176a36c42eafa452455de221cd052e4 100644 --- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java +++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/SharedMainParts.java @@ -99,6 +99,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa Runtime.getRuntime().addShutdownHook(new Thread(this::close)); exitCondition.await(); + logger.debug("After exit condition"); } public static String joinTopics(String... topics) { @@ -200,6 +201,7 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa logger.info("Exiting ..."); mainHandler.close(); model.ragconnectCloseConnections(); + logger.debug("After closing connections"); } public interface MqttHandlerWrapper<SELF> { diff --git a/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml index 32c3502e03b077ca3548d705ede80b8052bcc691..a9f039b27a6ff0e890fca5e5b595f6aa171c3053 100644 --- a/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml +++ b/ros3rag.placeA/src/main/resources/config-a-placeworld.yaml @@ -1,5 +1,5 @@ -#mqttHost: "192.168.0.122" -mqttHost: "localhost" +mqttHost: "192.168.0.122" +#mqttHost: "localhost" filenameRegions: "src/main/resources/regions-a-placeworld.json" forA: filenameInitialScene: "src/main/resources/config-scene-a-placeworld.json" diff --git a/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json index 968aaf8798b0ca57e6c7622d963e17373bca1301..404df52f51f0b66b874e61d911e7d2a3bdd0e7da 100644 --- a/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json +++ b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json @@ -1,4 +1,6 @@ { "objects": [ - { "id": "target","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, - { "id": "bigBlue","type": "BOX","pos": { "x": 8,"y": 3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } } + { "id": "target_Blue","type": "DROP_OFF_LOCATION","pos": { "x": 8,"y": 3,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "target_Green","type": "DROP_OFF_LOCATION","pos": { "x": 1,"y": 1,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "bigBlue","type": "BOX","pos": { "x": 8,"y": 3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "bigGreen","type": "BOX","pos": { "x": 1,"y": 1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } } ] } diff --git a/ros3rag.placeA/src/main/resources/regions-a-placeworld.json b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json index b07d06dba0de6016715e6e8aa6409b965b79c0c6..43cb170518c8f0e817ad26b47b719317f75a163d 100644 --- a/ros3rag.placeA/src/main/resources/regions-a-placeworld.json +++ b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json @@ -1,5 +1,6 @@ { "regions": [ - { "name": "G", "positions": ["target"] } + { "name": "G", "positions": ["target_Blue"] }, + { "name": "I", "positions": ["target_Green"] } ] } diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 9446b3d9392e90d1063b79c7eff711c21ebae4f5..e11e27bc4e2fe39072d0c30b81539ae9880150b9 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -309,32 +309,39 @@ aspect Computation { syn String Robot.myPosition() { //TODO "(^)" "(>)" RobotObject myRobotObject = myRobotObject(); + de.tudresden.inf.st.ceti.Object.State myState = myRobotObject != null ? myRobotObject.getState() : de.tudresden.inf.st.ceti.Object.State.STATE_UNKNOWN; + String pre = "eval of my pos for " + getName() + " .. current-pos: " + getCurrentPosition() + ", occ: '" + getOccupiedCollaborationZoneNames() + "', state: " + (myRobotObject != null ? myRobotObject.getState() : "null") + " -> "; for (String zoneName : arrayAsList(getOccupiedCollaborationZoneNames())) { if (zoneName.startsWith("(>)")) { String targetZone = zoneName.substring(3); - if (getCurrentPosition().equals(zoneName) && myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) { + if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) { + System.out.println(pre + targetZone); return targetZone; // robot was moving and has now reached its target } - if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) { + if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) { + System.out.println(pre + zoneName); return zoneName; // actually return (>) + targetZone } - // robot is not moving (or picking?) - return targetZone; } if (zoneName.startsWith("(^)")) { String targetZone = zoneName.substring(3); - if (getCurrentPosition().equals(zoneName) && myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) { + if (getCurrentPosition().equals(zoneName) && myState == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) { + System.out.println(pre + "(empty)"); return ""; // robot was moving and has now reached its target } - if (myRobotObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING) { + if (isStateMoving(myState) || "unknown".equals(getCurrentPosition())) { + System.out.println(pre + zoneName); return zoneName; // actually return (^) + targetZone } - // robot is not moving (or picking?) - return targetZone; } } + System.out.println(pre + getCurrentPosition()); return getCurrentPosition(); } + + private boolean Robot.isStateMoving(de.tudresden.inf.st.ceti.Object.State state) { + return state == de.tudresden.inf.st.ceti.Object.State.STATE_MOVING || state == de.tudresden.inf.st.ceti.Object.State.STATE_PICKING || state == de.tudresden.inf.st.ceti.Object.State.STATE_PLACING; + } } aspect AttributeMappings { @@ -434,6 +441,9 @@ aspect Navigation { } syn boolean Robot.isBusy() { + if (getCurrentPosition().startsWith("(")) { + return true; + } RobotObject obj = myRobotObject(); return obj != null && obj.getState() != de.tudresden.inf.st.ceti.Object.State.STATE_IDLE; } diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java index 49c01ed18d408139a1d37b82ad5b2a6eb8b8c260..2c0da972e5388c7a1adc87a845561efb05562ce7 100644 --- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/MainB.java @@ -1,9 +1,8 @@ package de.tudresden.inf.st.placeB; import de.tudresden.inf.st.ceti.Object; -import de.tudresden.inf.st.ceti.Reachability; +import de.tudresden.inf.st.placeB.ReachabilityConfiguration.RobotConfiguration; import de.tudresden.inf.st.placeB.ast.*; -import de.tudresden.inf.st.ros3rag.common.Configuration; import de.tudresden.inf.st.ros3rag.common.SharedMainParts; import de.tudresden.inf.st.ros3rag.common.Util; @@ -45,7 +44,8 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected void createSpecificMainHandlerConnections() { mainHandler.newConnection("place-b/demo", bytes -> { - switch (new String(bytes)) { + String topic = new String(bytes); + switch (topic) { case "objectRed1/red": UtilB.updatePositionOfObjectToLocation(model.getMyScene(), "objectRed1", "binRed"); break; @@ -57,17 +57,19 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { e.printStackTrace(); } break; - case "arm1/moving": - updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build()); - break; case "arm1/idle": - updateAndPublishScene("arm1", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build()); - break; - case "arm2/moving": - updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_MOVING).build()); - break; + case "arm1/picking": + case "arm1/placing": + case "arm1/moving": case "arm2/idle": - updateAndPublishScene("arm2", robot -> robot.toBuilder().setState(Object.State.STATE_IDLE).build()); + case "arm2/picking": + case "arm2/placing": + case "arm2/moving": + int slashIndex = topic.indexOf("/"); + String robot = topic.substring(0, slashIndex); + String stateString = "STATE_" + topic.substring(slashIndex + 1).toUpperCase(); + Object.State state = Object.State.valueOf(stateString); + updateAndPublishScene(robot, r -> r.toBuilder().setState(state).build()); break; case "big-blue/cz": updateAndPublishScene("bigBlue", bigBlue -> { @@ -90,7 +92,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { }); break; default: - logger.error("Unknown demo command {}", new String(bytes)); + logger.error("Unknown demo command {}", topic); } }); } @@ -116,15 +118,15 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { UtilB.setRegions(model, Util.parseRegionConfig(regionBFile)); // init robots and reachability - for (String reachabilityFileName : config.forB.reachability) { - // assumption: robots do not change during runtime, so we have stable connections - Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName)); - Reachability reachability = UtilB.readReachability(path); + // assumption: robots do not change during runtime, so we have stable connections + Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(config.forB.filenameReachability)); + ReachabilityConfiguration reachability = UtilB.readReachability(path.toFile()); - Robot robot = UtilB.createRobot(reachability.getIdRobot()); + for (RobotConfiguration robotConfiguration : reachability.robots) { + Robot robot = UtilB.createRobot(robotConfiguration.name); model.addRobot(robot); - CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); + CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convertToReachability(robotConfiguration.reachableLocations); robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); } } @@ -132,18 +134,45 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { @Override protected void connectEndpoints() throws IOException { for (String topic : config.forB.topicsSceneUpdate) { - model.connectMyScene(mqttUri(topic, config)); + checkSuccess( + model.connectMyScene(mqttUri(topic, config)), + "connectMyScene" + ); } - model.connectOtherScene(mqttUri("place-a/logical/update", config), 0); - model.connectNextOperation(mqttUri(config.forB.topicCommand, config), false); + checkSuccess( + model.connectOtherScene(mqttUri("place-a/logical/update", config), 0), + "OtherScene" + ); + checkSuccess( + model.connectNextOperation(mqttUri(config.forB.topicCommand, config), false), + "NextOperation" + ); for (Robot robot : model.getRobotList()) { // self-loop - robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)); - robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)); + checkSuccess( + robot.connectOwnedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)), + "OwnedCollaborationZoneNames (" + robot.getName() + ")" + ); + checkSuccess( + robot.connectOccupiedCollaborationZoneNames(mqttUri(config.forB.topicCommand, config)), + "OccupiedCollaborationZoneNames (" + robot.getName() + ")" + ); String topicPosition = joinTopics("place-b", robot.getName(), "position"); - robot.connectCurrentPosition(mqttUri(topicPosition, config)); - robot.connectMyPosition(mqttUri(topicPosition, config), true); + checkSuccess( + robot.connectCurrentPosition(mqttUri(topicPosition, config)), + "CurrentPosition (" + robot.getName() + ")" + ); + checkSuccess( + robot.connectMyPosition(mqttUri(topicPosition, config), true), + "MyPosition (" + robot.getName() + ")" + ); + } + } + + private void checkSuccess(boolean connectSuccess, String target) { + if (!connectSuccess) { + logger.warn("Did not connect successfully to {}", target); } } @@ -154,7 +183,7 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { builder.excludeChildren("Orientation", "Size"); builder.excludeRelations("ContainedInRegion"); builder.includeNonterminalAttributes("LogicalScene", "diffScenes", "diffToOperations"); - builder.includeAttributes("realRegion", "computeOperations"); + builder.includeAttributes("realRegion", "computeOperations", "myPosition"); builder.includeNullNodes(); }); mainHandler.publish(TOPIC_MODEL_SVG_PATH, filename.getBytes(StandardCharsets.UTF_8)); diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/ReachabilityConfiguration.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/ReachabilityConfiguration.java new file mode 100644 index 0000000000000000000000000000000000000000..015edb4d82a7a7b6e0d5d83ff53965a6a7a76516 --- /dev/null +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/ReachabilityConfiguration.java @@ -0,0 +1,17 @@ +package de.tudresden.inf.st.placeB; + +import java.util.List; + +/** + * Data class for reachability information. + * + * @author rschoene - Initial contribution + */ +public class ReachabilityConfiguration { + public List<RobotConfiguration> robots; + + public static class RobotConfiguration { + public String name; + public List<String> reachableLocations; + } +} 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 a83b918ec99c9224f6f1e14d5c5083c2018fafb0..926804a4b8afecbd5646827d9f58efbeadb7d3d6 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 @@ -2,6 +2,7 @@ package de.tudresden.inf.st.placeB; import de.tudresden.inf.st.ceti.Object; import de.tudresden.inf.st.ceti.Reachability; +import de.tudresden.inf.st.placeB.ReachabilityConfiguration.RobotConfiguration; import de.tudresden.inf.st.placeB.ast.*; import de.tudresden.inf.st.ros3rag.common.Configuration; import de.tudresden.inf.st.ros3rag.common.Util; @@ -13,7 +14,6 @@ import java.io.IOException; import java.nio.charset.StandardCharsets; import java.nio.file.Path; import java.nio.file.Paths; -import java.util.Arrays; import java.util.List; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; @@ -80,10 +80,7 @@ public class SimpleMainB { String filenameInitialSceneB = "src/main/resources/config-scene-b-" + scenario.suffix + ".json"; config.filenameRegions = "src/main/resources/regions-b-" + scenario.suffix + ".json"; config.forB = new Configuration.ConfigurationForB(); - config.forB.reachability = Arrays.asList( - "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json", - "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json" - ); + config.forB.filenameReachability = "src/main/resources/reachability-b-" + scenario.suffix + ".json"; Configuration configA = new Configuration(); String filenameInitialSceneA = "src/main/resources/config-scene-a-" + scenario.suffix + ".json"; @@ -128,15 +125,15 @@ public class SimpleMainB { UtilB.setRegions(model, Util.parseRegionConfig(regionBFile)); // init robots and reachability - for (String reachabilityFileName : config.forB.reachability) { - // assumption: robots do not change during runtime, so we have stable connections - Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityFileName)); - Reachability reachability = UtilB.readReachability(path); + // assumption: robots do not change during runtime, so we have stable connections + Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(config.forB.filenameReachability)); + ReachabilityConfiguration reachability = UtilB.readReachability(path.toFile()); + for (RobotConfiguration robotConfiguration : reachability.robots) { - Robot robot = UtilB.createRobot(reachability.getIdRobot()); + Robot robot = UtilB.createRobot(robotConfiguration.name); model.addRobot(robot); - CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); + CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convertToReachability(robotConfiguration.reachableLocations); robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); } @@ -274,29 +271,6 @@ public class SimpleMainB { } @SuppressWarnings("unused") - private void testRobotReachabilityBFS() throws Exception { - WorldModelB model = new WorldModelB(); - de.tudresden.inf.st.ceti.Scene scene = Util.readScene( - UtilB.pathToDirectoryOfPlaceB().resolve("src/main/resources/config-scene-b.json") - ); - Scene myScene = UtilB.convert(scene); - model.setMyScene(myScene); - for (String name : Util.extractRobotNames(scene)) { - Robot robot = UtilB.createRobot(name); - model.addRobot(robot); - String filenameReachability = "src/main/resources/dummy-reachability-b-" + name + ".json"; - Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(filenameReachability)); - Reachability reachability = UtilB.readReachability(path); - CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); - robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); - } - - printReachability(model); - printShortestPath(model, "binRed", "binGreen"); - printShortestPath(model, "binBlue", "binYellow"); - printShortestPath(model, "binRed", "binPurple"); - } - private void printReachability(WorldModelB model) { System.out.println("ModelInfos:"); System.out.println(UtilB.getModelInfos(model, true)); @@ -308,6 +282,7 @@ public class SimpleMainB { System.out.println(model.toReachabilityGraph().prettyPrint()); } + @SuppressWarnings("unused") private void printShortestPath(WorldModelB model, String source, String target) { LogicalRegion sourceLocation = model.getMyScene().getLogicalScene() .resolveLogicalObjectOfInterest(source).asLogicalRegion(); @@ -446,25 +421,6 @@ public class SimpleMainB { model.ragconnectCloseConnections(); } - @SuppressWarnings("unused") - private void testDummyReachability() throws Exception { - WorldModelB model = new WorldModelB(); - Robot arm1 = UtilB.createRobot("ARM1"); - model.addRobot(arm1); - Robot arm2 = UtilB.createRobot("ARM2"); - model.addRobot(arm2); - - Reachability reachabilityArm1 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB() - .resolve("src/main/resources/dummy-reachability-b-arm1.json")); - Reachability reachabilityArm2 = UtilB.readReachability(UtilB.pathToDirectoryOfPlaceB() - .resolve("src/main/resources/dummy-reachability-b-arm2.json")); - CanReachObjectOfInterestWrapper canReachForArm1 = UtilB.convert(reachabilityArm1); - CanReachObjectOfInterestWrapper canReachForArm2 = UtilB.convert(reachabilityArm2); - arm1.setCanReachObjectOfInterestWrapper(canReachForArm1); - arm2.setCanReachObjectOfInterestWrapper(canReachForArm2); - printModelInfos(model); - } - @SuppressWarnings("unused") private void testBuildModelB() { WorldModelB model = new WorldModelB(); diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java index 0619a5a9f6ae7263de48e9abbeb565beef2733fc..8e4b75ddda9809123c24ced67d23e7b4360b5b60 100644 --- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java @@ -1,5 +1,6 @@ package de.tudresden.inf.st.placeB; +import com.fasterxml.jackson.databind.ObjectMapper; import com.google.protobuf.util.JsonFormat; import de.tudresden.inf.st.ceti.Object; import de.tudresden.inf.st.ceti.Reachability; @@ -10,6 +11,7 @@ import de.tudresden.inf.st.ros3rag.common.Util; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; +import java.io.File; import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; @@ -24,12 +26,10 @@ import java.util.function.Function; public class UtilB { private static final Logger logger = LogManager.getLogger(UtilB.class); - static Reachability readReachability(Path path) throws IOException { - logger.debug("Reading reachability from {}", path.toAbsolutePath()); - var jsonString = Files.readString(path); - var builder = de.tudresden.inf.st.ceti.Reachability.newBuilder(); - JsonFormat.parser().merge(jsonString, builder); - return builder.build(); + static ReachabilityConfiguration readReachability(File reachabilityFile) throws IOException { + logger.info("Using reachability config file: {}", reachabilityFile.getAbsolutePath()); + ObjectMapper mapper = new ObjectMapper(); + return mapper.readValue(reachabilityFile, ReachabilityConfiguration.class); } static Path pathToDirectoryOfPlaceB() { @@ -37,7 +37,7 @@ public class UtilB { } static Robot createRobot(String name, String... canReach) { - Robot result = new Robot().setName(name); + Robot result = new Robot().setName(name).setCurrentPosition("unknown"); CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper(); for (String canReachName : canReach) { @@ -140,8 +140,10 @@ public class UtilB { return new ExposingASTNode().exposed_apply_ConvertScene(scene); } - static CanReachObjectOfInterestWrapper convert(de.tudresden.inf.st.ceti.Reachability r) throws Exception { - return new ExposingASTNode().exposed_apply_ConvertReachability(r); + static CanReachObjectOfInterestWrapper convertToReachability(List<String> reachableLocations) { + CanReachObjectOfInterestWrapper result = new CanReachObjectOfInterestWrapper(); + reachableLocations.forEach(location -> result.addCanReachObjectOfInterest(new CanReachObjectOfInterest(location))); + return result; } static void setRegions(WorldModelB model, RegionConfiguration config) { diff --git a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml index cd50a130378e62e501c2d8f429533bba5a52a13c..8045cd0feee0a59d265b557caefcbcd6436a1fde 100644 --- a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml +++ b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml @@ -1,12 +1,10 @@ -#mqttHost: "192.168.0.122" -mqttHost: "localhost" +mqttHost: "192.168.0.122" +#mqttHost: "localhost" filenameRegions: "src/main/resources/regions-b-placeworld.json" forB: topicsSceneUpdate: - "/ceti_cell_placeworld/scene/update" - "/ceti_cell_2_placeworld/scene/update" topicCommand: "/ceti_cell_placeworld/command" - reachability: - - "src/main/resources/reachability-b-arm1-placeworld.json" - - "src/main/resources/reachability-b-arm2-placeworld.json" + filenameReachability: "src/main/resources/reachability-b-placeworld.json" coordinatorMqttTopicPrefix: "coordinating/rag-b" diff --git a/ros3rag.placeB/src/main/resources/reachability-b-2022.json b/ros3rag.placeB/src/main/resources/reachability-b-2022.json new file mode 100644 index 0000000000000000000000000000000000000000..bac7298872018a6452ae35206a537d0980569065 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-2022.json @@ -0,0 +1,18 @@ +{ + "robots": [ + { + "name": "R1", + "reachableLocations": [ + "P1", "P3", "P5", + "P-E", "P-F" + ] + }, + { + "name": "R2", + "reachableLocations": [ + "P2", "P4", "P6", + "P-E", "P-F" + ] + } + ] +} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld-manual.json b/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld-manual.json deleted file mode 120000 index c75101cf0f5a6e367ac1dd9ac55ff0c81b2cfecf..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld-manual.json +++ /dev/null @@ -1 +0,0 @@ -reachability-b-arm1-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld.json b/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld.json deleted file mode 100644 index 0ac61c8a0aa95e363cb43564e5f71130e70bf205..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "idRobot": "arm1", - "objects": [ - { "idObject": "A1", "reachable": true }, - { "idObject": "B1", "reachable": true }, - { "idObject": "B2", "reachable": true }, - { "idObject": "C1", "reachable": true }, - { "idObject": "cz1", "reachable": true }, - { "idObject": "G1", "reachable": false } - ] -} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld-manual.json b/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld-manual.json deleted file mode 120000 index 678c8922d0785a2f854dc3f171944fb5172068cb..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld-manual.json +++ /dev/null @@ -1 +0,0 @@ -reachability-b-arm2-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld.json b/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld.json deleted file mode 100644 index acf74db8e4b378133a4f1abae5891f8276f6458b..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "idRobot": "arm2", - "objects": [ - { "idObject": "A1", "reachable": false }, - { "idObject": "B1", "reachable": false }, - { "idObject": "B2", "reachable": false }, - { "idObject": "C1", "reachable": false }, - { "idObject": "cz1", "reachable": true }, - { "idObject": "G1", "reachable": true } - ] -} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-mini.json b/ros3rag.placeB/src/main/resources/reachability-b-mini.json new file mode 100644 index 0000000000000000000000000000000000000000..7c6416dce2df703554994c3a1154900049b37d06 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-mini.json @@ -0,0 +1,18 @@ +{ + "robots": [ + { + "name": "R1", + "reachableLocations": [ + "P1", + "P-E" + ] + }, + { + "name": "R2", + "reachableLocations": [ + "P2.1", "P2.2", + "P-E" + ] + } + ] +} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-placeworld-manual.json b/ros3rag.placeB/src/main/resources/reachability-b-placeworld-manual.json new file mode 120000 index 0000000000000000000000000000000000000000..6032030ccdba4ad8a4d4b0776c57e3bd74f45574 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-placeworld-manual.json @@ -0,0 +1 @@ +reachability-b-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json b/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json new file mode 100644 index 0000000000000000000000000000000000000000..29f63f1969054177fca57102dc5f5988f54e169c --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-placeworld.json @@ -0,0 +1,21 @@ +{ + "robots": [ + { + "name": "arm1", + "reachableLocations": [ + "A1", + "B1", "B2", + "C1", + "cz1" + ] + }, + { + "name": "arm2", + "reachableLocations": [ + "cz1", + "G1", + "I1", "I2", "I3" ,"I4" + ] + } + ] +} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json b/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json deleted file mode 100644 index f8035b8c53da83df3f6525ae437f28a770734d52..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-r1-2022.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "idRobot": "R1", - "objects": [ - { "idObject": "O1", "reachable": true }, - { "idObject": "O2", "reachable": false }, - { "idObject": "O3", "reachable": true }, - { "idObject": "O4-uninvolved", "reachable": true }, - { "idObject": "O5-uninvolved", "reachable": true }, - { "idObject": "O6-uninvolved", "reachable": false }, - { "idObject": "P1", "reachable": true }, - { "idObject": "P2", "reachable": false }, - { "idObject": "P3", "reachable": true }, - { "idObject": "P4", "reachable": false }, - { "idObject": "P5", "reachable": true }, - { "idObject": "P6", "reachable": false }, - { "idObject": "P-E", "reachable": true }, - { "idObject": "P-F", "reachable": true } - ] -} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json b/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json deleted file mode 100644 index d63238f3e54a5d041efd2e89dcaad640c7a0d665..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-r1-mini.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "idRobot": "R1", - "objects": [ - { "idObject": "P1", "reachable": true }, - { "idObject": "P2.1", "reachable": false }, - { "idObject": "P2.2", "reachable": false }, - { "idObject": "P-E", "reachable": true } - ] -} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json b/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json deleted file mode 100644 index 319bfca072413d5aeb6ff03292c33bc9bf9e51ac..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-r2-2022.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "idRobot": "R2", - "objects": [ - { "idObject": "O1", "reachable": false }, - { "idObject": "O2", "reachable": true }, - { "idObject": "O3", "reachable": false }, - { "idObject": "O4-uninvolved", "reachable": false }, - { "idObject": "O5-uninvolved", "reachable": false }, - { "idObject": "O6-uninvolved", "reachable": true }, - { "idObject": "P1", "reachable": false }, - { "idObject": "P2", "reachable": true }, - { "idObject": "P3", "reachable": false }, - { "idObject": "P4", "reachable": true }, - { "idObject": "P5", "reachable": false }, - { "idObject": "P6", "reachable": true }, - { "idObject": "P-E", "reachable": true }, - { "idObject": "P-F", "reachable": true } - ] -} diff --git a/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json b/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json deleted file mode 100644 index 7d38b49803db68ec4c0d16ec8d0e54c39165c506..0000000000000000000000000000000000000000 --- a/ros3rag.placeB/src/main/resources/reachability-b-r2-mini.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "idRobot": "R1", - "objects": [ - { "idObject": "P1", "reachable": false }, - { "idObject": "P2.1", "reachable": true }, - { "idObject": "P2.2", "reachable": true }, - { "idObject": "P-E", "reachable": true } - ] -} diff --git a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json index da798ae978de102142d38f007f12e21309a3aa90..224d69e448eb070ba67c6beaa6795c783d352c84 100644 --- a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json +++ b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json @@ -3,8 +3,12 @@ { "name": "A", "positions": ["A1"] }, { "name": "B", "positions": ["B1", "B2"] }, { "name": "C", "positions": ["C1"] }, - { "name": "G", "positions": ["G1"] }, + { "name": "D", "positions": ["D1"] }, { "name": "E", "positions": ["E1"] }, + { "name": "F", "positions": ["F1"] }, + { "name": "G", "positions": ["G1"] }, + { "name": "H", "positions": ["H1"] }, + { "name": "I", "positions": ["I1", "I2", "I3", "I4"] }, { "name": "CZ", "positions": ["cz1"] } ] } diff --git a/ros3rag.scaling.b/src/main/java/de/tudresden/inf/st/scaling/b/MainScalingB.java b/ros3rag.scaling.b/src/main/java/de/tudresden/inf/st/scaling/b/MainScalingB.java index 41b8248dfcd92c784c477673d480e3df7a1d2113..bda361c9db92829fc99b36aefb1d5ab308b88984 100644 --- a/ros3rag.scaling.b/src/main/java/de/tudresden/inf/st/scaling/b/MainScalingB.java +++ b/ros3rag.scaling.b/src/main/java/de/tudresden/inf/st/scaling/b/MainScalingB.java @@ -128,7 +128,7 @@ public class MainScalingB { for (int robotIndex = 0; robotIndex < config.robots; robotIndex++) { myNewScene.addRobotObject(new RobotObject() .setName("ARM" + robotIndex) - .setActive(false) + .setState(de.tudresden.inf.st.ceti.Object.State.STATE_IDLE) .setPosition(new Position()) .setOrientation(new Orientation()) .setSize(new Size())); diff --git a/settings.gradle b/settings.gradle index 7f2471d0abbb86007f1f94a936b8111b71c39139..51000054cdeee2737c688c5666e0e3908265d149 100644 --- a/settings.gradle +++ b/settings.gradle @@ -10,5 +10,7 @@ include 'ros3rag.placeA' include 'ros3rag.placeB' include 'ros3rag.common' +include 'ros3rag.altPlaceB' + include 'ros3rag.scaling.a' include 'ros3rag.scaling.b'