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 new file mode 100644 index 0000000000000000000000000000000000000000..4323482606e8a6850c3304bb4f90fb3ff2919fb7 --- /dev/null +++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Configuration.java @@ -0,0 +1,23 @@ +package de.tudresden.inf.st.ros3rag.common; + +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; + +import java.util.List; + +/** + * New Data class for initial configuration. + * + * @author rschoene - Initial contribution + */ +@JsonIgnoreProperties(ignoreUnknown = true) +public class Configuration { + public String mqttHost; + public String filenameInitialScene; + public boolean useReachability; + 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/DataConfiguration.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java deleted file mode 100644 index 3c4b3d48f547119a081b02df494290cd39feb242..0000000000000000000000000000000000000000 --- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/DataConfiguration.java +++ /dev/null @@ -1,40 +0,0 @@ -package de.tudresden.inf.st.ros3rag.common; - -import com.fasterxml.jackson.annotation.JsonIgnoreProperties; - -import java.util.List; -import java.util.Map; -import java.util.SortedMap; - -/** - * Data class for initial configuration. - * - * @author rschoene - Initial contribution - */ -@JsonIgnoreProperties(ignoreUnknown = true) -public class DataConfiguration { - public ActualConfiguration panda_mqtt_connector; - @JsonIgnoreProperties(ignoreUnknown = true) - public static class ActualConfiguration { - public String server = "tcp://localhost:1883"; - public DataTopics topics; - public int zone_size; - public double robot_speed_factor; - public String robot_planning_mode = "fluid_path"; - public String default_trajectory = "<none>"; - public List<String> zones; - public Map<String, SortedMap<String, String>> parts; - public Map<String, SortedMap<String, String>> end_effectors; - public List<DataWorkPose> goal_poses; - } - public static class DataTopics { - public String robotConfig; - public String trajectory; - public String nextStep; - } - public static class DataWorkPose { - public String position; - public String orientation; - public String work; - } -} diff --git a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java index fce739f6c112659f3bbfc4a229e932fa4f6f5bb0..29c0d850fd2c370f5776469701314c9ac7c5c0d5 100644 --- a/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java +++ b/ros3rag.common/src/main/java/de/tudresden/inf/st/ros3rag/common/Util.java @@ -1,19 +1,20 @@ package de.tudresden.inf.st.ros3rag.common; import com.fasterxml.jackson.core.JsonParser; -import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; import com.fasterxml.jackson.dataformat.yaml.YAMLFactory; -import com.fasterxml.jackson.dataformat.yaml.YAMLParser; import com.google.protobuf.util.JsonFormat; -import de.tudresden.inf.st.ros3rag.common.DataConfiguration.ActualConfiguration; +import de.tudresden.inf.st.ceti.Object; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; import java.io.File; import java.io.IOException; -import java.net.URI; import java.nio.file.Files; -import java.util.Map; -import java.util.SortedMap; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; /** * Helper method dealing with config. @@ -21,59 +22,55 @@ import java.util.SortedMap; * @author rschoene - Initial contribution */ public class Util { + private static final Logger logger = LogManager.getLogger(Util.class); - public static ActualConfiguration parseConfig(File configFile) throws IOException { + public static Configuration parseConfig(File configFile) throws IOException { System.out.println("Using config file: " + configFile.getAbsolutePath()); ObjectMapper mapper = new ObjectMapper( new YAMLFactory().configure(JsonParser.Feature.ALLOW_YAML_COMMENTS, true) ); - ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector; - URI serverUri = URI.create(config.server); - config.server = serverUri.getHost() + ":" + (serverUri.getPort() == -1 ? 1883 : serverUri.getPort()) - + serverUri.getPath(); - return config; + return mapper.readValue(configFile, Configuration.class); } - public static void setMqttHost(SetHost handler, ActualConfiguration config) throws IOException { - HostAndPort hostAndPort = split(config.server); - handler.setHost(hostAndPort.host, hostAndPort.port); + public static String mqttUri(String topic, Configuration config) { + return "mqtt://" + config.mqttHost + "/" + topic; } - public static String mqttUri(String topic, ActualConfiguration config) { - return "mqtt://" + config.server + "/" + topic; + public static de.tudresden.inf.st.ceti.Scene readScene(java.nio.file.Path path) throws java.io.IOException { + logger.debug("Reading scene from {}", path.toAbsolutePath()); + var jsonString = Files.readString(path); + var builder = de.tudresden.inf.st.ceti.Scene.newBuilder(); + JsonFormat.parser().merge(jsonString, builder); + return builder.build(); } - public static void iterateLinks(HandleLink callback, ActualConfiguration config) throws IOException { - for (Map.Entry<String, SortedMap<String, String>> dataRobot : config.parts.entrySet()) { - String topicPrefix = dataRobot.getKey() + "/"; - for (Map.Entry<String, String> dataLink : dataRobot.getValue().entrySet()) { - String name = dataLink.getKey(); - callback.handle(false, topicPrefix + name, name); + public static List<String> extractRobotNames(de.tudresden.inf.st.ceti.Scene scene) { + List<String> result = new ArrayList<>(); + for (var obj : scene.getObjectsList()) { + if (obj.getType().equals(Object.Type.ARM)) { + result.add(obj.getId()); } } - for (Map.Entry<String, SortedMap<String, String>> dataRobot : config.end_effectors.entrySet()) { - String topicPrefix = dataRobot.getKey() + "/"; - for (Map.Entry<String, String> dataLink : dataRobot.getValue().entrySet()) { - String name = dataLink.getKey(); - callback.handle(true, topicPrefix + name, name); - } - } - } - - private static HostAndPort split(String serverString) { - HostAndPort result = new HostAndPort(); - String[] serverTokens = serverString.replace("tcp://", "").split(":"); - result.host = serverTokens[0]; - result.port = Integer.parseInt(serverTokens[1]); return result; } + public static Path pathToCommonDirectory() { + return pathToModuleDirectory("ros3rag.common"); + } - public static de.tudresden.inf.st.ceti.Scene readScene(java.nio.file.Path path) throws java.io.IOException { - var jsonString = Files.readString(path); - var builder = de.tudresden.inf.st.ceti.Scene.newBuilder(); - JsonFormat.parser().merge(jsonString, builder); - return builder.build(); + public static Path pathToModuleDirectory(String moduleName) { + Path current = Paths.get("").toAbsolutePath(); + String currentFileName = current.getFileName().toString(); + Path repoRoot; + if (currentFileName.equals("ros3rag.placeA") || currentFileName.equals("ros3rag.placeB") || currentFileName.equals("ros3rag.common")) { + // we are in some module, use parent + repoRoot = current.getParent(); + } else if (current.resolve(moduleName).toFile().exists()) { + repoRoot = current; + } else { + throw new IllegalStateException("Could not find ros3rag.common directory"); + } + return repoRoot.resolve(moduleName); } private static class HostAndPort { @@ -81,11 +78,6 @@ public class Util { int port; } - @FunctionalInterface - public interface SetHost { - void setHost(String host, int port) throws IOException; - } - @FunctionalInterface public interface HandleLink { void handle(boolean isEndEffector, String topic, String name) throws IOException; diff --git a/shared.place/jastadd/types.connect b/ros3rag.common/src/main/resources/jastadd/types.connect similarity index 74% rename from shared.place/jastadd/types.connect rename to ros3rag.common/src/main/resources/jastadd/types.connect index b885ea9ebc141955dd4a9db24020e290b1f777e6..9d2f860d5e7e2c3e2457a00d54fa763756137ecc 100644 --- a/shared.place/jastadd/types.connect +++ b/ros3rag.common/src/main/resources/jastadd/types.connect @@ -49,3 +49,17 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Scene {: } return result; :} + +ParseReachability maps byte[] bytes to de.tudresden.inf.st.ceti.Reachability {: + return de.tudresden.inf.st.ceti.Reachability.parseFrom(bytes); +:} + +ConvertReachability maps de.tudresden.inf.st.ceti.Reachability r to CanReachObjectOfInterestWrapper {: + var result = new CanReachObjectOfInterestWrapper(); + for (var objReach : r.getObjectsList()) { + if (objReach.getReachable()) { + result.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(objReach.getIdObject())); + } + } + return result; +:} diff --git a/shared.place/jastadd/types.jadd b/ros3rag.common/src/main/resources/jastadd/types.jadd similarity index 100% rename from shared.place/jastadd/types.jadd rename to ros3rag.common/src/main/resources/jastadd/types.jadd diff --git a/shared.place/jastadd/types.relast b/ros3rag.common/src/main/resources/jastadd/types.relast similarity index 92% rename from shared.place/jastadd/types.relast rename to ros3rag.common/src/main/resources/jastadd/types.relast index aa132b9836472180442bc421237eefd8748589f0..c99b52717463e2f61d3a4a3fb250844cbbd70faf 100644 --- a/shared.place/jastadd/types.relast +++ b/ros3rag.common/src/main/resources/jastadd/types.relast @@ -4,6 +4,7 @@ Orientation ::= <X:float> <Y:float> <Z:float> <W:float> ; Scene ::= DropOffLocation* MovableObject* /LogicalScene/ ; +CanReachObjectOfInterestWrapper ::= CanReachObjectOfInterest* ; ObjectOfInterest ::= <Name:String> Position Size Orientation ; DropOffLocation : ObjectOfInterest ::= ; diff --git a/shared.place/tasks.gradle b/ros3rag.common/src/main/resources/tasks.gradle similarity index 100% rename from shared.place/tasks.gradle rename to ros3rag.common/src/main/resources/tasks.gradle diff --git a/ros3rag.placeA/build.gradle b/ros3rag.placeA/build.gradle index c01971ace204cdf935aada1858ebcc75ce3eeda4..69f31e94c823d394dc6929e1b7145ba5c94bbe59 100644 --- a/ros3rag.placeA/build.gradle +++ b/ros3rag.placeA/build.gradle @@ -10,7 +10,14 @@ plugins { id 'ros3rag.java-ragconnect-conventions' } -mainClassName = 'de.tudresden.inf.st.placeA.SimpleMainA' +mainClassName = 'de.tudresden.inf.st.placeA.MainA' + +task simpleRun(type: JavaExec) { + group 'application' + classpath sourceSets.main.runtimeClasspath + main = "de.tudresden.inf.st.placeA.SimpleMainA" + +} dependencies { implementation project(':ros3rag.common') @@ -23,4 +30,4 @@ ext.ragConnectRootNode = 'WorldModelA' ext.relastFiles = ["src/gen/jastadd/WorldModelA.relast", "src/gen/jastadd/RagConnect.relast"] ext.jastaddAstPackage = 'de.tudresden.inf.st.placeA.ast' -apply from: '../shared.place/tasks.gradle' +apply from: '../ros3rag.common/src/main/resources/tasks.gradle' diff --git a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast index dee022c5a388a450470cfe0887928c746714c4fe..67c9b50130f87fa614a10a7381fb0ca6f72404dc 100644 --- a/ros3rag.placeA/src/main/jastadd/WorldModelA.relast +++ b/ros3rag.placeA/src/main/jastadd/WorldModelA.relast @@ -1,4 +1,4 @@ -WorldModelA ::= Robot Scene ; //Task* ; +WorldModelA ::= Robot [Scene] ; //Task* ; Robot ::= <Name:String> ; diff --git a/ros3rag.placeA/src/main/jastadd/shared b/ros3rag.placeA/src/main/jastadd/shared index 2b0e78b585f5c60aced5315df9bd94220deec252..b6cc005eb3c08ced15daeec938f4e7988fc9b732 120000 --- a/ros3rag.placeA/src/main/jastadd/shared +++ b/ros3rag.placeA/src/main/jastadd/shared @@ -1 +1 @@ -../../../../shared.place/jastadd/ \ No newline at end of file +../../../../ros3rag.common/src/main/resources/jastadd/ \ No newline at end of file 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 932100e808f449e65cc89f2ec68016c2fd4d2cc5..83ca62dd2629f663b2684111fd594b4c91d4e689 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 @@ -1,63 +1,106 @@ package de.tudresden.inf.st.placeA; import de.tudresden.inf.st.placeA.ast.MqttHandler; -import de.tudresden.inf.st.placeA.ast.Position; +import de.tudresden.inf.st.placeA.ast.Robot; +import de.tudresden.inf.st.placeA.ast.Scene; import de.tudresden.inf.st.placeA.ast.WorldModelA; -import de.tudresden.inf.st.ros3rag.common.DataConfiguration; 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.charset.StandardCharsets; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; +import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri; +import static de.tudresden.inf.st.ros3rag.common.Util.readScene; + /** - * TODO: Add description. + * Entry point for RAG model in place A. * * @author rschoene - Initial contribution */ public class MainA { - private static final Logger logger = LogManager.getLogger(MainA.class); + + private final static String TOPIC_MODEL = "place-a/model"; + private final static String TOPIC_STATUS = "place-a/status"; + private final static String TOPIC_EXIT = "place-a/exit"; + + private final static String TOPIC_SCENE_INIT = "place-a/init"; + private final static String TOPIC_SCENE_UPDATE_FROM_ROS = "place-a/scene/update"; + private final static String TOPIC_SCENE_UPDATE_TO_PLACE_B = "place-a/logical/update"; + private MqttHandler mainHandler; private WorldModelA model; - private void run(String[] args) throws IOException, InterruptedException { - System.out.println("This is place A with " + Position.of(1, 2, 3) + "!"); - File configFile = new File(args.length == 0 ? "../ros3rag.common/config.yaml" : args[0]); + public static void main(String[] args) throws Exception { + new MainA().run(args); + } + + private void run(String[] args) throws Exception { + String configFile = args.length == 0 ? "src/main/resources/config-a.yaml" : args[0]; // --- No configuration below this line --- - DataConfiguration.ActualConfiguration config = Util.parseConfig(configFile); + final var config = Util.parseConfig(UtilA.pathToDirectoryOfPlaceA().resolve(configFile).toFile()); model = new WorldModelA(); - logStatus("Start"); + /// Prepare main handler + mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage(); + mainHandler.setHost(config.mqttHost); CountDownLatch exitCondition = new CountDownLatch(1); + mainHandler.waitUntilReady(2, TimeUnit.SECONDS); + mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown()); + mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes))); - logger.info("To print the current model states, send a message to the topic 'model'."); - logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C."); + /// Reading scene and robot + de.tudresden.inf.st.ceti.Scene scene = readScene( + UtilA.pathToDirectoryOfPlaceA().resolve(config.filenameInitialScene) + ); + Scene myScene = UtilA.convert(scene); + model.setScene(myScene); - mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage(); - Util.setMqttHost(mainHandler::setHost, config); - mainHandler.waitUntilReady(2, TimeUnit.SECONDS); - mainHandler.newConnection("exit", bytes -> exitCondition.countDown()); - mainHandler.newConnection("model", bytes -> logStatus(new String(bytes))); + // if multiple robots are specified, then error message will be displayed + Util.extractRobotNames(scene).forEach(this::setRobot); + + /// Setup model connection + model.ragconnectCheckIncremental(); + model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); + + /// Connect endpoints + model.connectScene(mqttUri(TOPIC_SCENE_UPDATE_FROM_ROS, config)); + model.getScene().connectLogicalScene(mqttUri(TOPIC_SCENE_UPDATE_TO_PLACE_B, config), true); + + logStatus("Start"); -// sendInitialDataConfig(mainHandler, config.topics.dataConfig); + logger.info("To print the current model states, send a message to the topic '{}'.", TOPIC_MODEL); + logger.info("To exit the system cleanly, send a message to the topic '{}', or use Ctrl+C.", TOPIC_EXIT); + +// mainHandler.publish(mqttUri(TOPIC_SCENE_INIT, config), scene.toByteArray()); + logger.fatal("Skipping publishing to {} for now", TOPIC_SCENE_INIT); Runtime.getRuntime().addShutdownHook(new Thread(this::close)); exitCondition.await(); + } - this.close(); + private void setRobot(String name) { + if (model.getRobot() != null) { + logger.error("Robot already set. Overriding with newly found name."); + } + model.setRobot(new Robot().setName(name)); } private void logStatus(String prefix) { logger.info(prefix); - // more information to be added later + String content = UtilA.getModelInfos(model); + logger.info("WorldModelA\n{}", content); + if (mainHandler != null) { + mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8)); + } } private void close() { @@ -65,8 +108,4 @@ public class MainA { mainHandler.close(); model.ragconnectCloseConnections(); } - - public static void main(String[] args) throws IOException, InterruptedException { - new MainA().run(args); - } } diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java index b26dc9a86e794347e2599c9e44da693b49771c40..327b5b5ec5cab962a7f36108fc4125cb3dc0ac0f 100644 --- a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java +++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/SimpleMainA.java @@ -1,6 +1,5 @@ package de.tudresden.inf.st.placeA; -import com.google.protobuf.util.JsonFormat; import de.tudresden.inf.st.ceti.Object; import de.tudresden.inf.st.placeA.ast.*; import org.apache.commons.math3.geometry.euclidean.threed.Rotation; @@ -8,7 +7,6 @@ import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; -import java.nio.file.Files; import java.nio.file.Paths; import java.util.Scanner; import java.util.concurrent.TimeUnit; @@ -32,9 +30,9 @@ public class SimpleMainA { @SuppressWarnings("CommentedOutCode") private static void testReadSceneConfig() throws Exception { - final var path = Paths.get("..", "ros3rag.common", "src", "main", "resources", "config_scene.json"); + final var path = UtilA.pathToDirectoryOfPlaceA().resolve("src/main/resources/config-scene-a.json"); de.tudresden.inf.st.ceti.Scene scene = readScene(path); - Scene myScene = convert(scene); + Scene myScene = UtilA.convert(scene); var model = new WorldModelA(); model.setScene(myScene); @@ -64,16 +62,6 @@ public class SimpleMainA { model.getScene().connectLogicalScene("mqtt://localhost/logical/scene/update", true); } - @SuppressWarnings("rawtypes") - private static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception { - class ExposingASTNode extends ASTNode { - public Scene my_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception { - return ASTNode._apply_ConvertScene(pbScene); - } - } - return new ExposingASTNode().my_apply_ConvertScene(scene); - } - @SuppressWarnings("unused") private static void testBuildModelA() { WorldModelA model = new WorldModelA(); diff --git a/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java new file mode 100644 index 0000000000000000000000000000000000000000..c7910770b58214cc10b25f6b393c2f45e6e3ea6a --- /dev/null +++ b/ros3rag.placeA/src/main/java/de/tudresden/inf/st/placeA/UtilA.java @@ -0,0 +1,48 @@ +package de.tudresden.inf.st.placeA; + +import de.tudresden.inf.st.placeA.ast.ASTNode; +import de.tudresden.inf.st.placeA.ast.Scene; +import de.tudresden.inf.st.placeA.ast.WorldModelA; +import de.tudresden.inf.st.ros3rag.common.Util; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; + +import java.nio.file.Path; + +/** + * Static utility methods used only for place A. + * + * @author rschoene - Initial contribution + */ +public class UtilA { + private static final Logger logger = LogManager.getLogger(UtilA.class); + + static Path pathToDirectoryOfPlaceA() { + return Util.pathToModuleDirectory("ros3rag.placeA"); + } + + static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception { + return new ExposingASTNode().exposed_apply_ConvertScene(scene); + } + + public static String getModelInfos(WorldModelA model) { + StringBuilder sb = new StringBuilder(); + sb.append("myRobot: ") + .append(model.getRobot().getName()) + .append("\n"); + sb.append("myLogicalScene:"); + if (model.hasScene()) { + sb.append("\n").append(model.getScene().getLogicalScene().prettyPrint()); + } else { + sb.append(" (unset)\n"); + } + return sb.toString(); + } + + @SuppressWarnings("rawtypes") + static class ExposingASTNode extends ASTNode { + public Scene exposed_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception { + return ASTNode._apply_ConvertScene(pbScene); + } + } +} diff --git a/ros3rag.placeA/src/main/resources/config-a.yaml b/ros3rag.placeA/src/main/resources/config-a.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a3901c56e167bfdf68e837111068528b5ca991f2 --- /dev/null +++ b/ros3rag.placeA/src/main/resources/config-a.yaml @@ -0,0 +1,3 @@ +mqttHost: "localhost" +filenameInitialScene: "src/main/resources/config-scene-a.json" +useReachability: false diff --git a/ros3rag.common/src/main/resources/config_scene.json b/ros3rag.placeA/src/main/resources/config-scene-a.json similarity index 100% rename from ros3rag.common/src/main/resources/config_scene.json rename to ros3rag.placeA/src/main/resources/config-scene-a.json diff --git a/ros3rag.placeB/build.gradle b/ros3rag.placeB/build.gradle index 5060052d2dd3c58a2ad4e2fd7ddb99de85689f83..95d2d16ba73432095be516bff69b187c4fcd047f 100644 --- a/ros3rag.placeB/build.gradle +++ b/ros3rag.placeB/build.gradle @@ -11,7 +11,14 @@ plugins { id 'ros3rag.java-ragconnect-conventions' } -mainClassName = 'de.tudresden.inf.st.placeB.SimpleMainB' +mainClassName = 'de.tudresden.inf.st.placeB.MainB' + +task simpleRun(type: JavaExec) { + group 'application' + classpath sourceSets.main.runtimeClasspath + main = "de.tudresden.inf.st.placeB.SimpleMainB" + +} dependencies { implementation project(':ros3rag.common') @@ -24,4 +31,4 @@ ext.ragConnectRootNode = 'WorldModelB' ext.relastFiles = ["src/gen/jastadd/WorldModelB.relast", "src/gen/jastadd/RagConnect.relast"] ext.jastaddAstPackage = 'de.tudresden.inf.st.placeB.ast' -apply from: '../shared.place/tasks.gradle' +apply from: '../ros3rag.common/src/main/resources/tasks.gradle' diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect index a3464068b2863ba04cb3794c49a4ab2f7f445964..f77374792f0d58550ceb3515913b5a5a6f802752 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.connect +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.connect @@ -1,6 +1,7 @@ // --- receiving --- receive tree WorldModelB.MyScene using ParseScene, ConvertScene ; receive tree WorldModelB.OtherScene ; +receive tree Robot.CanReachObjectOfInterestWrapper using ParseReachability, ConvertReachability ; // --- sending --- send tree WorldModelB.NextOperation using PrintOperation ; diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd index 2dbc7570aa3eecc6b4bb8fad2fceda92ed3dce66..047ade07f567ab5e163fb7f04bace5daad17057b 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd @@ -74,7 +74,7 @@ aspect Computation { // find a location, that can be used as exchange point reached by two robots for (var robotForPrevious : robotsFitForPreviousLocation) { for (var robotForNew : robotsFitForNewLocation) { - for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestList()) { + for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) { String objectName = canReachOfRobotForPrevious.getObjectName(); if (resolveLogicalObjectOfInterest(objectName).isLogicalDropOffLocation() && robotForNew.canReach(objectName)) { // add two operations. @@ -109,7 +109,7 @@ aspect Computation { // syn List<String> locationIds syn boolean Robot.canReach(String objectName) { - for (var canReachObj : getCanReachObjectOfInterestList()) { + for (var canReachObj : getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) { if (canReachObj.getObjectName().equals(objectName)) { return true; } @@ -169,3 +169,14 @@ aspect Printing { return "+Error: " + getErrorMessage() + "+"; } } + +aspect Resolving { + public Optional<Robot> WorldModelB.findRobot(String name) { + for (Robot robot : getRobotList()) { + if (robot.getName().equals(name)) { + return Optional.of(robot); + } + } + return Optional.empty(); + } +} diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast index e6941e4cd11d0fc6a247e4cbae6e167d709f4126..6955828c67b6111c63c6d310571b6360a18d3ba2 100644 --- a/ros3rag.placeB/src/main/jastadd/WorldModelB.relast +++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.relast @@ -1,6 +1,6 @@ WorldModelB ::= Robot* [MyScene:Scene] [OtherScene:LogicalScene] /NextOperation:Operation/ ; -Robot ::= <Name:String> CanReachObjectOfInterest* ; +Robot ::= <Name:String> CanReachObjectOfInterestWrapper ; abstract Difference ; DifferenceObjectAtWrongPlace : Difference ; diff --git a/ros3rag.placeB/src/main/jastadd/shared b/ros3rag.placeB/src/main/jastadd/shared index 2b0e78b585f5c60aced5315df9bd94220deec252..b6cc005eb3c08ced15daeec938f4e7988fc9b732 120000 --- a/ros3rag.placeB/src/main/jastadd/shared +++ b/ros3rag.placeB/src/main/jastadd/shared @@ -1 +1 @@ -../../../../shared.place/jastadd/ \ No newline at end of file +../../../../ros3rag.common/src/main/resources/jastadd/ \ No newline at end of file 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 02bffb3abf8ff4f4af0c18501ed47bff1bd17f30..5548372b3bf8ec3a29bbfeab62ee46e292611885 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,12 +1,119 @@ package de.tudresden.inf.st.placeB; +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.Util; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; + +import java.io.File; +import java.nio.charset.StandardCharsets; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.TimeUnit; + +import static de.tudresden.inf.st.ros3rag.common.Util.mqttUri; +import static de.tudresden.inf.st.ros3rag.common.Util.readScene; + /** - * TODO: Add description. + * Entry point for RAG model in place B. * * @author rschoene - Initial contribution */ public class MainB { - public static void main(String[] args) { - System.out.println("This is another place B!"); + private static final Logger logger = LogManager.getLogger(SimpleMainB.class); + + private final static String TOPIC_MODEL = "place-b/model"; + private final static String TOPIC_STATUS = "place-b/status"; + private final static String TOPIC_EXIT = "place-b/exit"; + + private final static String TOPIC_MY_SCENE_INIT = "place-b/init"; + private final static String TOPIC_MY_SCENE_UPDATE_FROM_ROS = "place-b/scene/update"; + private final static String TOPIC_COMMANDS = "place-b/commands"; + + private final static String TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A = "place-a/logical/update"; + + private MqttHandler mainHandler; + private WorldModelB model; + + public static void main(String[] args) throws Exception { + new MainB().run(args); } + + private void run(String[] args) throws Exception { + String configFile = args.length == 0 ? "src/main/resources/config-b.yaml" : args[0]; + + // --- No configuration below this line --- + + final var config = Util.parseConfig(UtilB.pathToDirectoryOfPlaceB().resolve(configFile).toFile()); + model = new WorldModelB(); + + /// Prepare main handler + mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage(); + mainHandler.setHost(config.mqttHost); + mainHandler.waitUntilReady(2, TimeUnit.SECONDS); + CountDownLatch exitCondition = new CountDownLatch(1); + mainHandler.newConnection(TOPIC_EXIT, bytes -> exitCondition.countDown()); + mainHandler.newConnection(TOPIC_MODEL, bytes -> logStatus(new String(bytes))); + + /// 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); + Util.extractRobotNames(scene).forEach(name -> model.addRobot(UtilB.createRobot(name))); + + /// 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); + } + } + + /// Setup model connection + model.ragconnectCheckIncremental(); + model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); + + /// Connect endpoints + model.connectMyScene(mqttUri(TOPIC_MY_SCENE_UPDATE_FROM_ROS, config)); + model.connectOtherScene(mqttUri(TOPIC_OTHER_SCENE_UPDATE_FROM_PLACE_A, config)); + model.connectNextOperation(mqttUri(TOPIC_COMMANDS, config), false); + + logStatus("Start"); + + logger.info("To print the current model states, send a message to the topic '{}'.", TOPIC_MODEL); + logger.info("To exit the system cleanly, send a message to the topic '{}', or use Ctrl+C.", TOPIC_EXIT); + +// mainHandler.publish(mqttUri(TOPIC_SCENE_INIT, config), scene.toByteArray()); + logger.fatal("Skipping publishing to {} for now", TOPIC_MY_SCENE_INIT); + + Runtime.getRuntime().addShutdownHook(new Thread(this::close)); + + exitCondition.await(); + } + + private void logStatus(String message) { + logger.info(message); + String content = UtilB.getModelInfos(model); + logger.info("WorldModelB\n{}", content); + if (mainHandler != null) { + mainHandler.publish(TOPIC_STATUS, content.getBytes(StandardCharsets.UTF_8)); + } + } + + private void close() { + logger.info("Exiting ..."); + mainHandler.close(); + model.ragconnectCloseConnections(); + } + } 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 d00c6a20bd74a73a32ac51624fb9a1f220c69fcf..20b29d23ca5e2cb583a4e6ae15616304d26507c4 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 @@ -1,15 +1,17 @@ package de.tudresden.inf.st.placeB; +import com.google.protobuf.util.JsonFormat; +import de.tudresden.inf.st.ceti.Reachability; import de.tudresden.inf.st.placeB.ast.*; import de.tudresden.inf.st.ros3rag.common.Util; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; +import java.io.IOException; +import java.nio.charset.StandardCharsets; +import java.nio.file.Files; +import java.nio.file.Path; import java.nio.file.Paths; -import java.util.concurrent.CountDownLatch; -import java.util.concurrent.TimeUnit; - -import static de.tudresden.inf.st.ros3rag.common.Util.readScene; /** * Testing features for placeB. @@ -18,83 +20,44 @@ import static de.tudresden.inf.st.ros3rag.common.Util.readScene; */ public class SimpleMainB { private static final Logger logger = LogManager.getLogger(SimpleMainB.class); - private MqttHandler mainHandler; - private WorldModelB model; public static void main(String[] args) throws Exception { + System.out.println("Running SimpleMainB"); new SimpleMainB().run(args); } + @SuppressWarnings({"unused", "CommentedOutCode"}) private void run(String[] args) throws Exception { // testBuildModelB(); - testReceivingModelB(); +// testReceivingModelB(); + testReachability(); } - private void testReceivingModelB() throws Exception { - final var path = Paths.get("..", "ros3rag.common", "src", "main", "resources", "config_scene.json"); - - model = new WorldModelB(); - - de.tudresden.inf.st.ceti.Scene scene = readScene(path); - Scene myScene = convert(scene); - model.setMyScene(myScene); - - model.ragconnectCheckIncremental(); - model.ragconnectSetupMqttWaitUntilReady(2, TimeUnit.SECONDS); - - model.connectOtherScene("mqtt://localhost/logical/scene/update"); - - logStatus("Start"); - CountDownLatch exitCondition = new CountDownLatch(1); - - logger.info("To print the current model states, send a message to the topic 'model'."); - logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C."); - - mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage(); -// Util.setMqttHost(mainHandler::setHost, config); - mainHandler.waitUntilReady(2, TimeUnit.SECONDS); - mainHandler.newConnection("exit", bytes -> exitCondition.countDown()); - mainHandler.newConnection("model", bytes -> logStatus(new String(bytes))); - -// sendInitialDataConfig(mainHandler, config.topics.dataConfig); - - Runtime.getRuntime().addShutdownHook(new Thread(this::close)); - - exitCondition.await(); - - this.close(); - } - - private void logStatus(String message) { - logger.info(message); - // more information to be added later + private void testReachability() 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); } - private void close() { - logger.info("Exiting ..."); - mainHandler.close(); - model.ragconnectCloseConnections(); - } - - - @SuppressWarnings("rawtypes") - private static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception { - class ExposingASTNode extends ASTNode { - public Scene my_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception { - return ASTNode._apply_ConvertScene(pbScene); - } - } - return new ExposingASTNode().my_apply_ConvertScene(scene); - } - @SuppressWarnings("unused") private void testBuildModelB() { WorldModelB model = new WorldModelB(); // robots - model.addRobot(createRobot("Alice", "placeAlfa", "placeBeta")); - model.addRobot(createRobot("Bob", "placeBeta", "placeGamma")); + model.addRobot(UtilB.createRobot("Alice", "placeAlfa", "placeBeta")); + model.addRobot(UtilB.createRobot("Bob", "placeBeta", "placeGamma")); // myScene var myScene = new Scene(); @@ -143,23 +106,7 @@ public class SimpleMainB { printModelInfos(model); } - private static void printModelInfos(WorldModelB model) { - logger.info("myRobots: {}", model.getRobotList().prettyPrint( - robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestList().prettyPrint(CanReachObjectOfInterest::getObjectName) + ")" - )); - logger.info("myLogicalScene:\n{}", model.getMyScene().getLogicalScene().prettyPrint()); - logger.info("otherScene:\n{}", model.getOtherScene().prettyPrint()); - logger.info("Diff: {}", model.diffScenes().prettyPrint()); - logger.info("Operations: {}", model.diffToOperations().prettyPrint()); - logger.info("Next operation: {}", model.getNextOperation()); - } - - private static Robot createRobot(String name, String... canReach) { - Robot result = new Robot() - .setName(name); - for (String canReachName : canReach) { - result.addCanReachObjectOfInterest(new CanReachObjectOfInterest(canReachName)); - } - return result; + private void printModelInfos(WorldModelB model) { + logger.info(UtilB.getModelInfos(model)); } } 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 new file mode 100644 index 0000000000000000000000000000000000000000..80b67ac249e4fefbae14e04e19d965c63a6877b2 --- /dev/null +++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/UtilB.java @@ -0,0 +1,92 @@ +package de.tudresden.inf.st.placeB; + +import com.google.protobuf.util.JsonFormat; +import de.tudresden.inf.st.ceti.Object; +import de.tudresden.inf.st.ceti.Reachability; +import de.tudresden.inf.st.placeB.ast.*; +import de.tudresden.inf.st.ros3rag.common.Util; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; + +import java.io.IOException; +import java.nio.charset.StandardCharsets; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.List; + +/** + * Static utility methods used only for place B. + * + * @author rschoene - Initial contribution + */ +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 Path pathToDirectoryOfPlaceB() { + return Util.pathToModuleDirectory("ros3rag.placeB"); + } + + static Robot createRobot(String name, String... canReach) { + Robot result = new Robot().setName(name); + CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper(); + + for (String canReachName : canReach) { + wrapper.addCanReachObjectOfInterest(new CanReachObjectOfInterest(canReachName)); + } + result.setCanReachObjectOfInterestWrapper(wrapper); + return result; + } + + static String getModelInfos(WorldModelB model) { + StringBuilder sb = new StringBuilder(); + sb.append("myRobots: ") + .append(model.getRobotList().prettyPrint( + robot -> robot.getName() + "(canReach: " + robot.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList().prettyPrint(CanReachObjectOfInterest::getObjectName) + ")")) + .append("\n"); + sb.append("myLogicalScene:"); + if (model.hasMyScene()) { + sb.append("\n").append(model.getMyScene().getLogicalScene().prettyPrint()); + } else { + sb.append(" (unset)\n"); + } + sb.append("otherScene:"); + if (model.hasOtherScene()) { + sb.append("\n").append(model.getOtherScene().prettyPrint()); + } else { + sb.append(" (unset)\n"); + } + sb.append("Diff: ").append(model.diffScenes().prettyPrint()).append("\n"); + sb.append("Operations: ").append(model.diffToOperations().prettyPrint()).append("\n"); + sb.append("Next operation: ").append(model.getNextOperation()).append("\n"); + return sb.toString(); + } + + static Scene convert(de.tudresden.inf.st.ceti.Scene scene) throws Exception { + 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); + } + + @SuppressWarnings("rawtypes") + static class ExposingASTNode extends ASTNode { + public Scene exposed_apply_ConvertScene(de.tudresden.inf.st.ceti.Scene pbScene) throws Exception { + return ASTNode._apply_ConvertScene(pbScene); + } + public CanReachObjectOfInterestWrapper exposed_apply_ConvertReachability(de.tudresden.inf.st.ceti.Reachability r) throws Exception { + return ASTNode._apply_ConvertReachability(r); + } + } + +} diff --git a/ros3rag.placeB/src/main/resources/config-b.yaml b/ros3rag.placeB/src/main/resources/config-b.yaml new file mode 100644 index 0000000000000000000000000000000000000000..91aa9417a2e917f521e4a87944d9f96efd78cb34 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/config-b.yaml @@ -0,0 +1,8 @@ +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" diff --git a/ros3rag.placeB/src/main/resources/config-scene-b.json b/ros3rag.placeB/src/main/resources/config-scene-b.json new file mode 100644 index 0000000000000000000000000000000000000000..f14e17fe05da23db662d514b49ae47d406a0ddd4 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/config-scene-b.json @@ -0,0 +1,21 @@ +{ "objects": [ + { "id": "tablePillar1","pos": { "x": 0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar2","pos": { "x": -0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar3","pos": { "x": -0.77,"y": 0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "tablePillar4","pos": { "x": 0.77,"y": -0.77,"z": 0.325 },"size": { "length": 0.06,"width": 0.06,"height": 0.65 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "table","pos": { "z": 0.7 },"size": { "length": 1.6,"width": 1.6,"height": 0.1 },"orientation": { "w": 1 },"color": { "r": 255,"g": 222,"b": 173 } }, + { "id": "binBlue","type": "BIN","pos": { "x": -0.34,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "b": 1 } }, + { "id": "binRed","type": "BIN","pos": { "x": 0.06,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "binGreen","type": "BIN","pos": { "x": 0.46,"y": 0.49,"z": 0.8325 },"size": { "length": 0.21,"width": 0.3,"height": 0.165 },"orientation": { "w": 1 },"color": { "g": 1 } }, + { "id": "objectRed1","type": "BOX","pos": { "x": 0.5,"y": -0.1,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "objectRed2","type": "BOX","pos": { "x": 0.25,"y": -0.2,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "w": 1 },"color": { "r": 1 } }, + { "id": "objectRed3","type": "BOX","pos": { "x": -0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "r": 1 } }, + { "id": "objectGreen1","type": "BOX","pos": { "x": 0.45,"y": -0.3,"z": 0.8105 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "objectGreen2","type": "BOX","pos": { "x": -0.45,"y": -0.2,"z": 0.8105 },"size": {"length":0.031,"width":0.031,"height":0.138},"orientation": { "z": 1,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "objectGreen3","type": "BOX","pos": { "x": 0.1,"y": -0.3,"z": 0.819 },"size": {"length":0.031,"width":0.062,"height":0.121},"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "g": 1 } }, + { "id": "objectBlue1","type": "BOX","pos": { "x": 0.25,"y": -0.4,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } }, + { "id": "objectBlue2","type": "BOX","pos": { "x": -0.3,"y": -0.3,"z": 0.8105 },"size": { "length": 0.031,"width": 0.062,"height": 0.121 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } }, + { "id": "objectBlue3","type": "BOX","pos": { "x": 0.4,"z": 0.819 },"size": { "length": 0.031,"width": 0.031,"height": 0.138 },"orientation": { "z": 0.382683,"w": 0.92388 },"color": { "b": 1 } }, + { "id": "arm1","type": "ARM","pos": { "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } }, + { "id": "arm2","type": "ARM","pos": { "x": 1, "z": 0.75 },"size": { },"orientation": { "w": 1 },"color": { "r": 1.00,"g": 1.00,"b": 1.00 } } +] } diff --git a/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json new file mode 100644 index 0000000000000000000000000000000000000000..d2380778cefd608719724b34fc476164a042a1b1 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm1.json @@ -0,0 +1,11 @@ +{ + "idRobot": "ARM1", + "objects": [ + { "idObject": "objectRed1", "reachable": true }, + { "idObject": "objectRed2", "reachable": true }, + { "idObject": "objectRed3", "reachable": false }, + { "idObject": "binRed", "reachable": true }, + { "idObject": "binBlue", "reachable": true }, + { "idObject": "binGreen", "reachable": false } + ] +} diff --git a/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json new file mode 100644 index 0000000000000000000000000000000000000000..c058487fd15694cfff2fb1f0fbaeead9d54cb308 --- /dev/null +++ b/ros3rag.placeB/src/main/resources/dummy-reachability-b-arm2.json @@ -0,0 +1,11 @@ +{ + "idRobot": "ARM2", + "objects": [ + { "idObject": "objectRed1", "reachable": false }, + { "idObject": "objectRed2", "reachable": false }, + { "idObject": "objectRed3", "reachable": false }, + { "idObject": "binRed", "reachable": false }, + { "idObject": "binBlue", "reachable": true }, + { "idObject": "binGreen", "reachable": true } + ] +}