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 a0ac80bcc753c4681a07f04103de219a586b0996..daf8b294566302d65ce457c146d095e34eedcb0a 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 @@ -12,14 +12,12 @@ import java.util.List; @JsonIgnoreProperties(ignoreUnknown = true) public class Configuration { public String mqttHost; - public String filenameInitialScene; + public String filenameInitialSceneA; public String filenameRegions; - public boolean useReachability; public String coordinatorMqttTopicPrefix; public List<ReachabilityConfig> reachability; public static class ReachabilityConfig { - public String idRobot; public String filename; } 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 2f443ae90408c20d5bf34a5a85b471cd910c059d..020482130b220a30c8e424563ef63af994c4fef8 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 @@ -1,6 +1,5 @@ package de.tudresden.inf.st.ros3rag.common; -import de.tudresden.inf.st.ceti.Scene; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -26,7 +25,6 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa private final String TOPIC_STATUS; private final String TOPIC_REWIND; private final String TOPIC_EXIT; - private final String TOPIC_SCENE_INIT; private static final String TOPIC_SUFFIX_COORDINATOR_STATUS = "status"; private static final String TOPIC_SUFFIX_COORDINATOR_COMMAND = "command"; @@ -46,12 +44,8 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa this.TOPIC_STATUS = cellName + "/status"; this.TOPIC_REWIND = cellName + "/rewind"; this.TOPIC_EXIT = cellName + "/exit"; - - this.TOPIC_SCENE_INIT = cellName + "/scene/init"; } - protected abstract MqttHandler createMqttHandler(); - private Map<String, String> channelDescriptions() { return new LinkedHashMap<>() {{ put(TOPIC_MODEL, "Print current model (detailed if message starts with 'detail')"); @@ -137,8 +131,6 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa } } - protected abstract void createSpecificMainHandlerConnections(); - private void rewind(String statusMessage) throws Exception { if (model != null) { logger.debug("Closing previous connections for {}", cellName); @@ -147,8 +139,8 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa logger.debug("Creating world model for {}", cellName); model = createWorldModel(); - logger.debug("Reading scene for {}", cellName); - Scene scene = readSceneAndRobots(); + logger.debug("Reading robots for {}", cellName); + readRobotsAndReachability(); logger.debug("Setup model connection for {}", cellName); model.ragconnectCheckIncremental(); @@ -167,7 +159,6 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa logStatus(statusMessage); - mainHandler.publish(TOPIC_SCENE_INIT, scene.toByteArray()); if (config.useCoordinator()) { logger.debug("Publishing ready for {}", cellName); mainHandler.publish(joinTopics(config.coordinatorMqttTopicPrefix, TOPIC_SUFFIX_COORDINATOR_STATUS), @@ -175,11 +166,15 @@ public abstract class SharedMainParts<MqttHandler extends SharedMainParts.MqttHa } } - protected abstract Scene readSceneAndRobots() throws Exception; + protected abstract MqttHandler createMqttHandler(); - protected abstract void connectEndpoints() throws IOException; + protected abstract void createSpecificMainHandlerConnections(); + + protected abstract WorldModel createWorldModel() throws Exception; - protected abstract WorldModel createWorldModel(); + protected abstract void readRobotsAndReachability() throws Exception; + + protected abstract void connectEndpoints() throws IOException; protected abstract String getModelInfos(WorldModel model, boolean detailed); diff --git a/ros3rag.common/src/main/resources/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd index 91f383e5b77ee77e32903579e8f1321bdf294f6c..11404ecf542e3c1ad69bf2cd0c5613a3eec0d03e 100644 --- a/ros3rag.common/src/main/resources/jastadd/types.jadd +++ b/ros3rag.common/src/main/resources/jastadd/types.jadd @@ -40,7 +40,8 @@ aspect Resolving { syn DropOffLocation LogicalMovableObject.myLocation() { // classical reference attribute (was relation, but not allowed with RagConnect) - return containingScene().resolveObjectOfInterest(getNameOfMyLocation()).asDropOffLocation(); + ObjectOfInterest obj = containingScene().resolveObjectOfInterest(getNameOfMyLocation()); + return obj == null ? null : obj.asDropOffLocation(); } } @@ -66,7 +67,7 @@ aspect Computation { // we use half of the size of to the position as it is in the center of the location double halfLocationLength = 0.5 * locationSize.getLength(); double halfLocationWidth = 0.5 * locationSize.getWidth(); - double halfLocationHeight = 0.5 * locationSize.getHeight(); +// double halfLocationHeight = 0.5 * locationSize.getHeight(); double rotatedX = rotatedObjectVector.getX(); double rotatedY = rotatedObjectVector.getY(); @@ -80,8 +81,8 @@ aspect Computation { // halfLocationHeight); return LT(-halfLocationLength, rotatedX) && LT(rotatedX, halfLocationLength) && - LT(-halfLocationWidth, rotatedY) && LT(rotatedY, halfLocationWidth) && - LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight); + LT(-halfLocationWidth, rotatedY) && LT(rotatedY, halfLocationWidth); // && + //LT(-halfLocationHeight, rotatedZ) && LT(rotatedZ, halfLocationHeight); } syn MovableObject DropOffLocation.getObjectLocatedHere() { diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java index 195f2aac572ab7b80b6e321967b22668ba9ec365..2dfe78585660895e756e7f4eab2a8d71017c7505 100644 --- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java +++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/MainA.java @@ -38,6 +38,11 @@ public class MainA extends SharedMainParts<MqttHandler, WorldModelA> { new MainA(configFile).run(); } + @Override + protected MqttHandler createMqttHandler() { + return new MqttHandler("mainHandlerA"); + } + @Override protected void createSpecificMainHandlerConnections() { mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_BLUE, bytes -> @@ -55,17 +60,17 @@ public class MainA extends SharedMainParts<MqttHandler, WorldModelA> { } @Override - protected de.tudresden.inf.st.ceti.Scene readSceneAndRobots() throws Exception { - /// Reading scene + protected WorldModelA createWorldModel() throws Exception { de.tudresden.inf.st.ceti.Scene scene = readScene( - UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene) + UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialSceneA) ); Scene myScene = UtilA.convert(scene); - model.setScene(myScene); + return new WorldModelA().setScene(myScene); + } + @Override + protected void readRobotsAndReachability() { // no robots to be set - - return scene; } @Override @@ -74,16 +79,6 @@ public class MainA extends SharedMainParts<MqttHandler, WorldModelA> { model.connectLogicalScene(mqttUri(TOPIC_SCENE_UPDATE_TO_PLACE_B, config), true); } - @Override - protected MqttHandler createMqttHandler() { - return new MqttHandler("mainHandlerA"); - } - - @Override - protected WorldModelA createWorldModel() { - return new WorldModelA(); - } - @Override protected String getModelInfos(WorldModelA worldModelA, boolean detailed) { return UtilA.getModelInfos(model, detailed); diff --git a/ros3rag.placeA/src/main/resources/config-a.yaml b/ros3rag.placeA/src/main/resources/config-a.yaml index ab5d9af69a148c11b82c1f3301c8b0e2727bad18..723ecb2f25cc6097aa73452fe5ffb98e3df73ad9 100644 --- a/ros3rag.placeA/src/main/resources/config-a.yaml +++ b/ros3rag.placeA/src/main/resources/config-a.yaml @@ -1,4 +1,3 @@ mqttHost: "localhost" -filenameInitialScene: "src/main/resources/config-scene-a.json" -useReachability: false +filenameInitialSceneA: "src/main/resources/config-scene-a.json" coordinatorMqttTopicPrefix: "coordinating/rag-a" diff --git a/ros3rag.placeA/src/main/resources/config-scene-a-placeworld-manual.json b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld-manual.json new file mode 120000 index 0000000000000000000000000000000000000000..48833ba117b882b8a3e830e9e76e6da8a5cad4ab --- /dev/null +++ b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld-manual.json @@ -0,0 +1 @@ +config-scene-a-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json new file mode 100644 index 0000000000000000000000000000000000000000..968aaf8798b0ca57e6c7622d963e17373bca1301 --- /dev/null +++ b/ros3rag.placeA/src/main/resources/config-scene-a-placeworld.json @@ -0,0 +1,4 @@ +{ "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 } } +] } diff --git a/ros3rag.placeA/src/main/resources/regions-a-placeworld-manual.json b/ros3rag.placeA/src/main/resources/regions-a-placeworld-manual.json new file mode 120000 index 0000000000000000000000000000000000000000..470dce03963a7f931b30f35c136cef7ff0582a06 --- /dev/null +++ b/ros3rag.placeA/src/main/resources/regions-a-placeworld-manual.json @@ -0,0 +1 @@ +regions-a-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeA/src/main/resources/regions-a-placeworld.json b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json new file mode 100644 index 0000000000000000000000000000000000000000..b07d06dba0de6016715e6e8aa6409b965b79c0c6 --- /dev/null +++ b/ros3rag.placeA/src/main/resources/regions-a-placeworld.json @@ -0,0 +1,5 @@ +{ + "regions": [ + { "name": "G", "positions": ["target"] } + ] +} diff --git a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag index 97a95cb4cab6ade325a28991ebacbec5fea4b608..11b245a8ce76d5c05cb76824e8bec90cc6d991af 100644 --- a/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag +++ b/ros3rag.placeB/src/main/jastadd/RobotReachabilityToBFS.jrag @@ -24,9 +24,10 @@ aspect RobotReachabilityToBFS { continue; } List<DropOffLocation> reachableLocations = robot.reachableObjects().stream() - .filter(ObjectOfInterest::isDropOffLocation) - .map(ObjectOfInterest::asDropOffLocation) - .collect(java.util.stream.Collectors.toList()); + .filter(Objects::nonNull) + .filter(ObjectOfInterest::isDropOffLocation) + .map(ObjectOfInterest::asDropOffLocation) + .collect(java.util.stream.Collectors.toList()); // bidirectional edges between locations by default // unidirectional edges from all occupied locations to free locations // no edge if both locations are occupied diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 3d7f79e1b31b282b4d17bf4ed575a79b1363bfd0..0980bd050593b753314bfd92483eeaa467c1b8c5 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -350,7 +350,10 @@ aspect Navigation { return result; } - syn boolean Robot.isBusy() = worldModelB().getMyScene().resolveObjectOfInterest(getName()).asRobotObject().getActive(); + syn boolean Robot.isBusy() { + ObjectOfInterest obj = worldModelB().getMyScene().resolveObjectOfInterest(getName()); + return obj == null ? false : obj.asRobotObject().getActive(); + } } aspect GlueForShared { 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 88ca30aea2761657ebabae442a1e8b95ef474a93..4095335721e53b3e5bf4a67d5a35425901f9130a 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 @@ -4,14 +4,12 @@ import de.tudresden.inf.st.ceti.Reachability; 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; import java.io.IOException; import java.nio.file.Path; import java.nio.file.Paths; import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri; -import static de.tudresden.inf.st.ros3rag.common.Util.readScene; /** * Entry point for RAG model in place B. @@ -19,7 +17,6 @@ import static de.tudresden.inf.st.ros3rag.common.Util.readScene; * @author rschoene - Initial contribution */ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { - private final String TOPIC_ROBOT_REACHABILITY; private final String TOPIC_MY_SCENE_UPDATE_FROM_ROS; private final String TOPIC_COMMAND; private final String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A; @@ -27,7 +24,6 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { MainB(String configFile) { super("place-b", UtilB.pathToDirectoryOfPlaceB().resolve(configFile)); - this.TOPIC_ROBOT_REACHABILITY = cellName + "/reachability/%s"; this.TOPIC_MY_SCENE_UPDATE_FROM_ROS = cellName + "/scene/update"; this.TOPIC_COMMAND = cellName + "/command"; @@ -41,6 +37,11 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { new MainB(configFile).run(); } + @Override + protected MqttHandler createMqttHandler() { + return new MqttHandler("mainHandlerB"); + } + @Override protected void createSpecificMainHandlerConnections() { mainHandler.newConnection(TOPIC_DEMO_MOVE_objectRed1_RED, bytes -> @@ -49,34 +50,28 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { } @Override - protected de.tudresden.inf.st.ceti.Scene readSceneAndRobots() throws Exception { - /// Reading scene and robots - de.tudresden.inf.st.ceti.Scene scene = readScene( - UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene) - ); - Scene myScene = UtilB.convert(scene); - model.setMyScene(myScene); - for (String name : Util.extractRobotNames(scene)) { - Robot robot = UtilB.createRobot(name); - model.addRobot(robot); + protected WorldModelB createWorldModel() { + WorldModelB result = new WorldModelB(); + result.addOtherScene(new LogicalScene()); + return result; + } + + @Override + protected void readRobotsAndReachability() throws Exception { + model.setMyScene(new Scene()); + + // init robots and reachability + for (Configuration.ReachabilityConfig reachabilityConfig : config.reachability) { // assumption: robots do not change during runtime, so we have stable connections - robot.connectCanReachObjectOfInterestWrapper(mqttUri(String.format(TOPIC_ROBOT_REACHABILITY, name), config)); - } + Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename)); + Reachability reachability = UtilB.readReachability(path); - /// Set (dummy) reachability, if configured - if (config.useReachability) { - for (Configuration.ReachabilityConfig reachabilityConfig : config.reachability) { - Robot robot = model.findRobot(reachabilityConfig.idRobot).orElseThrow( - () -> new IllegalArgumentException("Could not find robot with name " + reachabilityConfig.idRobot) - ); - Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename)); - Reachability reachability = UtilB.readReachability(path); - CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); - robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); - } - } + Robot robot = UtilB.createRobot(reachability.getIdRobot()); + model.addRobot(robot); - return scene; + CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); + robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); + } } @Override @@ -84,18 +79,10 @@ public class MainB extends SharedMainParts<MqttHandler, WorldModelB> { model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config)); model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config), 0); model.connectNextOperation(mqttUri(TOPIC_COMMAND, config), false); - } - - @Override - protected MqttHandler createMqttHandler() { - return new MqttHandler("mainHandlerB"); - } - - @Override - protected WorldModelB createWorldModel() { - WorldModelB result = new WorldModelB(); - result.addOtherScene(new LogicalScene()); - return result; + for (Robot robot : model.getRobotList()) { + // self-loop + robot.connectOwnedCollaborationZoneNames(mqttUri(TOPIC_COMMAND, config)); + } } @Override 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 281acccf95245e6cefc049543cebe8988669182d..41630bec39373a6684d2970f79249428d5e106ab 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 @@ -28,8 +28,43 @@ import java.util.stream.Collectors; */ public class SimpleMainB { private static final Logger logger = LogManager.getLogger(SimpleMainB.class); - boolean exitAutomatically = true; - boolean useMini = true; + + static class Scenario { + final String suffix; + final boolean loadAndChangeScenes; + final boolean exitAutomatically; + final String mqttHost; + final String topicSceneUpdateB; + final String topicCommand; + Scenario(String suffix) { + this(suffix, true, true, + "localhost", "place-b/scene/update", "place-b/command"); + } + Scenario(String suffix, boolean loadAndChangeScenes, boolean exitAutomatically, + String mqttHost, String topicSceneUpdateB, String topicCommand) { + this.suffix = suffix; + this.loadAndChangeScenes = loadAndChangeScenes; + this.exitAutomatically = exitAutomatically; + this.mqttHost = mqttHost; + this.topicSceneUpdateB = topicSceneUpdateB; + this.topicCommand = topicCommand; + } + } + Scenario s2022 = new Scenario("2022"); + Scenario sMini = new Scenario("mini"); + Scenario sPlaceworld = new Scenario("placeworld", false, false, + "192.168.0.122", + "/ceti_cell_placeworld/scene/update", + "/ceti_cell_placeworld/command"); + Scenario sPlaceworldManual = new Scenario("placeworld-manual", false, false, + "localhost", + "/ceti_cell_placeworld/scene/update", + "/ceti_cell_placeworld/command"); + + @SuppressWarnings("unused" ) + Scenario[] allScenarios = new Scenario[] { s2022, sMini, sPlaceworld, sPlaceworldManual }; + + final Scenario scenario = sPlaceworldManual; public static void main(String[] args) throws Exception { System.out.println("Running SimpleMainB"); @@ -42,31 +77,29 @@ public class SimpleMainB { } private void readModelAndReceiveFromA() throws Exception { - final String suffix = useMini ? "mini" : "2022"; - Configuration config = new Configuration(); - config.mqttHost = "localhost"; - config.filenameInitialScene = "src/main/resources/config-scene-b-" + suffix + ".json"; - config.filenameRegions = "src/main/resources/regions-b-" + suffix + ".json"; + config.mqttHost = scenario.mqttHost; + String filenameInitialSceneB = "src/main/resources/config-scene-b-" + scenario.suffix + ".json"; + config.filenameRegions = "src/main/resources/regions-b-" + scenario.suffix + ".json"; ReachabilityConfig reachabilityR1 = new ReachabilityConfig(); - reachabilityR1.idRobot = "R1"; - reachabilityR1.filename = "src/main/resources/reachability-b-r1-" + suffix + ".json"; + reachabilityR1.filename = "src/main/resources/reachability-b-r1-" + scenario.suffix + ".json"; ReachabilityConfig reachabilityR2 = new ReachabilityConfig(); - reachabilityR2.idRobot = "R2"; - reachabilityR2.filename = "src/main/resources/reachability-b-r2-" + suffix + ".json"; + reachabilityR2.filename = "src/main/resources/reachability-b-r2-" + scenario.suffix + ".json"; config.reachability = Arrays.asList(reachabilityR1, reachabilityR2); Configuration configA = new Configuration(); - configA.filenameInitialScene = "src/main/resources/config-scene-a-" + suffix + ".json"; - configA.filenameRegions = "src/main/resources/regions-a-" + suffix + ".json"; + String filenameInitialSceneA = "src/main/resources/config-scene-a-" + scenario.suffix + ".json"; + configA.filenameRegions = "src/main/resources/regions-a-" + scenario.suffix + ".json"; - final String topicSceneUpdateB = "place-b/scene/update"; + final String topicSceneUpdateB = scenario.topicSceneUpdateB; final String topicUpdateFromPlaceA = "update/logical/fromA"; - final String topicCommand = "place-b/command"; + final String topicCommand = scenario.topicCommand; final String topicExit = "place-b/exit"; final String topicModel = "place-b/model"; final String topicModelStatus = "place-b/status"; + logger.info("Using scenario {}", scenario.suffix); + WorldModelB model = new WorldModelB(); model.addOtherScene(new LogicalScene()); @@ -75,30 +108,36 @@ public class SimpleMainB { mqttHandler.waitUntilReady(2, TimeUnit.SECONDS); model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); + Scene myScene; // read initial scene - Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameInitialScene); - de.tudresden.inf.st.ceti.Scene scene = Util.readScene(initialSceneFile); - Scene myScene = UtilB.convert(scene); + Path initialSceneFile = UtilB.pathToDirectoryOfPlaceB().resolve(filenameInitialSceneB); + de.tudresden.inf.st.ceti.Scene initialScene = Util.readScene(initialSceneFile); + if (scenario.loadAndChangeScenes) { + myScene = UtilB.convert(initialScene); + } else { + myScene = new Scene(); + + mqttHandler.newConnection("coordinating/rag-b/command", bytes -> { + if (new String(bytes).equals("start")) { + mqttHandler.publish(scenario.topicSceneUpdateB, initialScene.toByteArray()); + } + }); + } model.setMyScene(myScene); // read and set regions File regionBFile = UtilB.pathToDirectoryOfPlaceB().resolve(config.filenameRegions).toFile(); UtilB.setRegions(model, Util.parseRegionConfig(regionBFile)); - // init robots (copied from MainB) - for (String name : Util.extractRobotNames(scene)) { - Robot robot = UtilB.createRobot(name); - model.addRobot(robot); - // assumption: robots do not change during runtime, so we have stable connections - } - - // init reachability (copied from MainB) + // init robots and reachability for (ReachabilityConfig reachabilityConfig : config.reachability) { - Robot robot = model.findRobot(reachabilityConfig.idRobot).orElseThrow( - () -> new IllegalArgumentException("Could not find robot with id " + reachabilityConfig.idRobot) - ); + // assumption: robots do not change during runtime, so we have stable connections Path path = UtilB.pathToDirectoryOfPlaceB().resolve(Paths.get(reachabilityConfig.filename)); Reachability reachability = UtilB.readReachability(path); + + Robot robot = UtilB.createRobot(reachability.getIdRobot()); + model.addRobot(robot); + CanReachObjectOfInterestWrapper reachabilityWrapper = UtilB.convert(reachability); robot.setCanReachObjectOfInterestWrapper(reachabilityWrapper); } @@ -138,7 +177,7 @@ public class SimpleMainB { }); // read scene from A, serialize it and send it via mqtt - Path sceneAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve(configA.filenameInitialScene); + Path sceneAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve(filenameInitialSceneA); File regionAFile = Util.pathToModuleDirectory("ros3rag.placeA").resolve(configA.filenameRegions).toFile(); de.tudresden.inf.st.ceti.Scene scenePlaceA = Util.readScene(sceneAFile); Scene sceneFromPlaceA = UtilB.convert(scenePlaceA); @@ -149,39 +188,47 @@ public class SimpleMainB { LogicalScene logicalSceneFromPlaceA = sceneFromPlaceA.getLogicalScene(); byte[] bytesToSend = _ragconnect__apply__TreeDefaultLogicalSceneToBytesMapping(logicalSceneFromPlaceA); - describedWait(1, "send new logical scene"); - mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend); + if (scenario.loadAndChangeScenes) { + describedWait(1, "send new logical scene" ); + mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend); - describedWait(2, "print model status"); - mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + describedWait(2, "print model status" ); + mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); - // set object O1 to position of P-E in sceneB and publish it - describedWait(3, "send updated sceneB (P-E)"); - scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P-E").getPosition()); - mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); + // set object O1 to position of P-E in sceneB and publish it + describedWait(3, "send updated sceneB (P-E)"); + de.tudresden.inf.st.ceti.Scene scene = updatePositionOfObject(initialScene, "O1", myScene.resolveObjectOfInterest("P-E").getPosition()); + mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); - describedWait(4, "set R1 to not busy"); - scene = updateNotBusyOfRobot(scene, "R1"); + describedWait(4, "set R1 to not busy"); + scene = updateNotBusyOfRobot(scene, "R1"); - describedWait(5, "print model status"); - mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + describedWait(5, "print model status"); + mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); - // set object O1 to position of P2.2 in sceneB and publish it - describedWait(6, "send updated sceneB (P2.2)"); - scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P2.2").getPosition()); - mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); + // set object O1 to position of P2.2 in sceneB and publish it + describedWait(6, "send updated sceneB (P2.2)"); + scene = updatePositionOfObject(scene, "O1", myScene.resolveObjectOfInterest("P2.2").getPosition()); + mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); - describedWait(7, "print model status"); - mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + describedWait(7, "print model status"); + mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); - describedWait(8, "set R2 to not busy"); - scene = updateNotBusyOfRobot(scene, "R2"); - mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); + describedWait(8, "set R2 to not busy"); + scene = updateNotBusyOfRobot(scene, "R2"); + mqttHandler.publish(topicSceneUpdateB, scene.toByteArray()); - describedWait(9, "print model status"); - mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + describedWait(9, "print model status"); + mqttHandler.publish(topicModel, "detailed".getBytes(StandardCharsets.UTF_8)); + } else { + mqttHandler.newConnection("coordinating/rag-a/command", bytes -> { + if (new String(bytes).equals("start")) { + mqttHandler.publish(topicUpdateFromPlaceA, bytesToSend); + } + }); + } - if (exitAutomatically) { + if (scenario.exitAutomatically) { TimeUnit.SECONDS.sleep(5); exitCondition.countDown(); } @@ -197,7 +244,7 @@ public class SimpleMainB { static de.tudresden.inf.st.ceti.Scene updatePositionOfObject( de.tudresden.inf.st.ceti.Scene scene, - String objectName, + @SuppressWarnings("SameParameterValue" ) String objectName, Position newPosition) { return updateObject(scene, objectName, obj -> { Object.Builder builder = obj.toBuilder(); @@ -218,8 +265,7 @@ public class SimpleMainB { static de.tudresden.inf.st.ceti.Scene updateObject( de.tudresden.inf.st.ceti.Scene scene, String objectName, - Function<Object, Object> change - ) { + Function<Object, Object> change) { List<Object> objectsList = scene.getObjectsList(); Object newObj = null; int index, objectsListSize; diff --git a/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml new file mode 100644 index 0000000000000000000000000000000000000000..c6d99b60cd735534c3ade8b42a7a6d767bd17fac --- /dev/null +++ b/ros3rag.placeB/src/main/resources/config-b-placeworld.yaml @@ -0,0 +1,6 @@ +mqttHost: "localhost" +useReachability: true +reachability: + - filename: "src/main/resources/reachability-b-arm1-placeworld.json" + - filename: "src/main/resources/reachability-b-arm2-placeworld.json" +coordinatorMqttTopicPrefix: "coordinating/rag-b" diff --git a/ros3rag.placeB/src/main/resources/config-b.yaml b/ros3rag.placeB/src/main/resources/config-b.yaml index 5f1f4ebc981fa7420f1598f53ba143391722b7b3..7cee63eb7154fa4c35599a7824b9420020d9f7b4 100644 --- a/ros3rag.placeB/src/main/resources/config-b.yaml +++ b/ros3rag.placeB/src/main/resources/config-b.yaml @@ -1,11 +1,6 @@ mqttHost: "localhost" -filenameInitialScene: "src/main/resources/config-scene-b.json" -useReachability: true reachability: - - idRobot: "arm1" - filename: "src/main/resources/dummy-reachability-b-arm1.json" - - idRobot: "arm2" - filename: "src/main/resources/dummy-reachability-b-arm2.json" - - idRobot: "arm3" - filename: "src/main/resources/dummy-reachability-b-arm3.json" + - filename: "src/main/resources/dummy-reachability-b-arm1.json" + - filename: "src/main/resources/dummy-reachability-b-arm2.json" + - filename: "src/main/resources/dummy-reachability-b-arm3.json" coordinatorMqttTopicPrefix: "coordinating/rag-b" diff --git a/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json b/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json new file mode 100644 index 0000000000000000000000000000000000000000..0de3c1e3ccca49a71a69636aea3cf25fe02c3bd6 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/config-scene-b-placeworld-manual.json @@ -0,0 +1,1522 @@ +{ + "objects": [ + { + "id": "table1_wheel_1", + "pos": { + "x": 0.28, + "y": 0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_wheel_2", + "pos": { + "x": -0.28, + "y": -0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_wheel_3", + "pos": { + "x": -0.28, + "y": 0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_wheel_4", + "pos": { + "x": 0.28, + "y": -0.28, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_body_front", + "pos": { + "x": 0.345, + "z": 0.5 + }, + "size": { + "length": 0.005, + "width": 0.69, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_body_back", + "pos": { + "x": -0.345, + "z": 0.45 + }, + "size": { + "length": 0.005, + "width": 0.69, + "height": 0.66 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_body_left", + "pos": { + "y": 0.345, + "z": 0.5 + }, + "size": { + "length": 0.69, + "width": 0.005, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_body_right", + "pos": { + "y": -0.345, + "z": 0.5 + }, + "size": { + "length": 0.69, + "width": 0.005, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_table_top", + "pos": { + "z": 0.885 + }, + "size": { + "length": 0.8, + "width": 0.8, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_left_panel", + "pos": { + "z": 0.885, + "y": -0.6525 + }, + "size": { + "length": 0.7, + "width": 0.5, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table1_front_panel", + "pos": { + "z": 0.885, + "x": 0.6525 + }, + "size": { + "length": 0.5, + "width": 0.7, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "shared_panel", + "pos": { + "z": 0.885, + "y": 0.6525 + }, + "size": { + "length": 0.7, + "width": 0.5, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_wheel_1", + "pos": { + "x": 0.28, + "y": 1.59, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_wheel_2", + "pos": { + "x": -0.28, + "y": 1.03, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_wheel_3", + "pos": { + "x": -0.28, + "y": 1.59, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_wheel_4", + "pos": { + "x": 0.28, + "y": 1.03, + "z": 0.06 + }, + "size": { + "length": 0.12, + "width": 0.12, + "height": 0.12 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_body_front", + "pos": { + "x": 0.345, + "y": 1.31, + "z": 0.5 + }, + "size": { + "length": 0.005, + "width": 0.69, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_body_back", + "pos": { + "x": -0.345, + "y": 1.31, + "z": 0.45 + }, + "size": { + "length": 0.005, + "width": 0.69, + "height": 0.66 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_body_left", + "pos": { + "y": 1.655, + "z": 0.5 + }, + "size": { + "length": 0.69, + "width": 0.005, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_body_right", + "pos": { + "y": 0.965, + "z": 0.5 + }, + "size": { + "length": 0.69, + "width": 0.005, + "height": 0.76 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_table_top", + "pos": { + "y": 1.31, + "z": 0.885 + }, + "size": { + "length": 0.8, + "width": 0.8, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_front_panel", + "pos": { + "x": 0.6525, + "y": 1.31, + "z": 0.885 + }, + "size": { + "length": 0.5, + "width": 0.7, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "table2_right_panel", + "pos": { + "z": 0.885, + "y": 1.9625 + }, + "size": { + "length": 0.7, + "width": 0.5, + "height": 0.01 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 0.15, + "g": 0.15, + "b": 0.15 + } + }, + { + "id": "A1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": -0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "A2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": -0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "A3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": -0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "A4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": -0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "B1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": -0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "B2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": -0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": -0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": -0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": -0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": -0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C5", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": -0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C6", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": -0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C7", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": -0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C8", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": -0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "C9", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": -0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D5", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D6", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D7", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 0.1, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D8", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 0.2, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "D9", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 0.3, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "E1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": 0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "E2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": 0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "E3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": 0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "E4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": 0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "F1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 0.6, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "F2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 0.7, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.11, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.21, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G5", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.11, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G6", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.21, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G7", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G8", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.11, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "G9", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.21, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.41, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.51, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.61, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.41, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H5", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.51, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H6", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.61, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H7", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.41, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H8", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.51, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "H9", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.3, + "y": 1.61, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "I1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": 1.91, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "I2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.3, + "y": 2.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "I3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": 1.91, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "I4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": -0.2, + "y": 2.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "J1", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 1.91, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "J2", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.1, + "y": 2.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "J3", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 1.91, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "J4", + "type": "DROP_OFF_LOCATION", + "pos": { + "x": 0.2, + "y": 2.01, + "z": 0.89 + }, + "size": { + "length": 0.1, + "width": 0.1, + "height": 0 + }, + "orientation": { + "w": 1 + } + }, + { + "id": "cz1", + "type": "COLLABORATION_ZONE", + "pos": { + "z": 1.49, + "y": 0.6525 + }, + "size": { + "length": 0.7, + "width": 0.3, + "height": 1.2 + }, + "orientation": { + "w": 1 + }, + "color": { + "r": 1, + "g": 0.627, + "b": 0 + } + }, + { + "id": "bigBlue", + "type": "BOX", + "pos": { + "x": 0.1, + "y": -0.25, + "z": 0.9355 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.091 + }, + "orientation": { + "w": 1 + }, + "color": { + "b": 1 + } + }, + { + "id": "bigGreen", + "type": "BOX", + "pos": { + "x": -0.3, + "y": 2.01, + "z": 0.9355 + }, + "size": { + "length": 0.0318, + "width": 0.0636, + "height": 0.091 + }, + "orientation": { + "w": 1 + }, + "color": { + "g": 1 + } + }, + { + "id": "arm1", + "type": "ARM", + "pos": { + "x": -0.22, + "z": 0.89 + }, + "size": {}, + "orientation": { + "w": 1 + }, + "color": { + "r": 1, + "g": 1, + "b": 1 + } + }, + { + "id": "arm2", + "type": "ARM", + "pos": { + "x": -0.22, + "y": 1.31, + "z": 0.89 + }, + "size": {}, + "orientation": { + "w": 1 + }, + "color": { + "r": 1, + "g": 1, + "b": 1 + } + } + ] +} 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 new file mode 120000 index 0000000000000000000000000000000000000000..c75101cf0f5a6e367ac1dd9ac55ff0c81b2cfecf --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld-manual.json @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000000000000000000000000000000000000..0ac61c8a0aa95e363cb43564e5f71130e70bf205 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-arm1-placeworld.json @@ -0,0 +1,11 @@ +{ + "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 new file mode 120000 index 0000000000000000000000000000000000000000..678c8922d0785a2f854dc3f171944fb5172068cb --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld-manual.json @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000000000000000000000000000000000000..acf74db8e4b378133a4f1abae5891f8276f6458b --- /dev/null +++ b/ros3rag.placeB/src/main/resources/reachability-b-arm2-placeworld.json @@ -0,0 +1,11 @@ +{ + "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/regions-b-placeworld-manual.json b/ros3rag.placeB/src/main/resources/regions-b-placeworld-manual.json new file mode 120000 index 0000000000000000000000000000000000000000..9adc3eb65536c6674834cb7f4329b6e4ba960ddc --- /dev/null +++ b/ros3rag.placeB/src/main/resources/regions-b-placeworld-manual.json @@ -0,0 +1 @@ +regions-b-placeworld.json \ No newline at end of file diff --git a/ros3rag.placeB/src/main/resources/regions-b-placeworld.json b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json new file mode 100644 index 0000000000000000000000000000000000000000..19abce0fd735d0a9a7aea30be33e21b6e005f617 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/regions-b-placeworld.json @@ -0,0 +1,9 @@ +{ + "regions": [ + { "name": "A", "positions": ["A1"] }, + { "name": "B", "positions": ["B1", "B2"] }, + { "name": "C", "positions": ["C1"] }, + { "name": "G", "positions": ["G1"] }, + { "name": "E", "positions": ["E1"] } + ] +}