diff --git a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java b/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java index f954120848dad5a83d498f91c6c33bdceb32969f..f4a635989356bb01b2940e3ca93d76bd034e98ba 100644 --- a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java +++ b/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java @@ -4,9 +4,10 @@ import com.fasterxml.jackson.databind.ObjectMapper; import com.google.protobuf.InvalidProtocolBufferException; import config.Dataconfig.DataConfig; import config.Robotconfig.RobotConfig; +import de.tudresden.inf.st.ros2rag.starter.Util; import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater; import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration; -import de.tudresden.inf.st.ros2rag.starter.data.DataJoint; +import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration.ActualConfiguration; import org.apache.logging.log4j.Level; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; @@ -16,6 +17,7 @@ import java.io.File; import java.io.IOException; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; +import java.util.concurrent.atomic.AtomicInteger; public class Main { @@ -27,23 +29,24 @@ public class Main { ObjectMapper mapper = new ObjectMapper(); File configFile = new File(args[0]); System.out.println("Using config file: " + configFile.getAbsolutePath()); - DataConfiguration config = mapper.readValue(configFile, DataConfiguration.class); + ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector; main.run(config); } - private void run(DataConfiguration config) throws IOException, InterruptedException { + private void run(ActualConfiguration config) throws IOException, InterruptedException { final CountDownLatch finish = new CountDownLatch(1); - int topicMaxLength = 0; + AtomicInteger topicMaxLength = new AtomicInteger(); MqttUpdater receiver = new MqttUpdater("receiver stub"); - receiver.setHost(config.mqttHost); + Util.setMqttHost(receiver, config); receiver.waitUntilReady(2, TimeUnit.SECONDS); - receiver.newConnection(config.robotConfigTopic, this::printRobotConfig); - receiver.newConnection(config.dataConfigTopic, this::printDataConfig); - for (DataJoint joint : config.joints) { - receiver.newConnection(joint.topic, this::printPandaLinkState); - topicMaxLength = Math.max(topicMaxLength, joint.topic.length()); - } + receiver.newConnection(config.topics.robotConfig, this::printRobotConfig); + receiver.newConnection(config.topics.dataConfig, this::printDataConfig); + + Util.iterateLinks((isEndEffector, topic, name) -> { + receiver.newConnection(topic, this::printPandaLinkState); + topicMaxLength.set(Math.max(topicMaxLength.get(), topic.length())); + }, config); this.topicPattern = "%" + topicMaxLength + "s"; receiver.newConnection("components", bytes -> { @@ -59,11 +62,7 @@ public class Main { }); Runtime.getRuntime().addShutdownHook(new Thread(receiver::close)); - if (config.exitAfterSeconds > 0) { - finish.await(config.exitAfterSeconds, TimeUnit.SECONDS); - } else { - finish.await(); - } + finish.await(); receiver.close(); } diff --git a/ros2rag.starter/build.gradle b/ros2rag.starter/build.gradle index 9a29c6de9d4efd433e3eef0ff10fa203e89d6bbd..b4aec98a67e1ddc6657e293bb90d4d9dfdf5ec29 100644 --- a/ros2rag.starter/build.gradle +++ b/ros2rag.starter/build.gradle @@ -30,6 +30,7 @@ dependencies { baseRuntimeClasspath project (':ros2rag.base') implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-xml', version: "${jackson_version}" implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: "${jackson_version}" + implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_version}" implementation group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11' implementation group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0' implementation group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15' diff --git a/ros2rag.starter/src/main/jastadd/Computation.jrag b/ros2rag.starter/src/main/jastadd/Computation.jrag index bddac5a4db5245259f19cfb72c86b9533fbf3fa7..d7c003c29cffe8d885430c929c0ffa89ccc514b3 100644 --- a/ros2rag.starter/src/main/jastadd/Computation.jrag +++ b/ros2rag.starter/src/main/jastadd/Computation.jrag @@ -1,8 +1,8 @@ aspect Computation { syn boolean RobotArm.isInSafetyZone() { System.out.println("isInSafetyZone()"); - for (Joint joint : getJointList()) { - if (model().getZoneModel().isInSafetyZone(joint.getCurrentPosition())) { + for (Link link : getLinkList()) { + if (model().getZoneModel().isInSafetyZone(link.getCurrentPosition())) { return true; } } diff --git a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag index 60e7d1923a9a0a735bc4c8c3718ddd1c8e5b7ed7..77225e9fb6abf9c91c11dd1b45adf22ff8ef5ff4 100644 --- a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag +++ b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag @@ -2,11 +2,11 @@ * Version 2020-05-28 */ // --- update definitions --- -read Joint.CurrentPosition using ParseLinkState, LinkStateToIntPosition ; +read Link.CurrentPosition using ParseLinkState, LinkStateToIntPosition ; write RobotArm.AppropriateSpeed using CreateSpeedMessage, SerializeRobotConfig ; // --- dependency definitions --- -RobotArm.AppropriateSpeed canDependOn Joint.CurrentPosition as dependency1 ; +RobotArm.AppropriateSpeed canDependOn Link.CurrentPosition as dependency1 ; // --- mapping definitions --- ParseLinkState maps byte[] bytes to panda.Linkstate.PandaLinkState {: diff --git a/ros2rag.starter/src/main/jastadd/Navigation.jrag b/ros2rag.starter/src/main/jastadd/Navigation.jrag index 855a2d87d6bbb1374c5ae23569435fe2a9b329ac..db53288f4c4b61f0c5d315f8a18ab3f5bac9b720 100644 --- a/ros2rag.starter/src/main/jastadd/Navigation.jrag +++ b/ros2rag.starter/src/main/jastadd/Navigation.jrag @@ -2,7 +2,7 @@ aspect Navigation { inh Model RobotArm.model(); eq Model.getRobotArm().model() = this; - inh RobotArm Joint.containingRobotArm(); - eq RobotArm.getJoint().containingRobotArm() = this; + inh RobotArm Link.containingRobotArm(); + eq RobotArm.getLink().containingRobotArm() = this; eq RobotArm.getEndEffector().containingRobotArm() = this; } diff --git a/ros2rag.starter/src/main/jastadd/RobotModel.relast b/ros2rag.starter/src/main/jastadd/RobotModel.relast index 415d3e5e25b8d03fa1d53bc6f95c05ad5e1987d8..b8d932ae470e4bf3dc433f8610eec1f05016e595 100644 --- a/ros2rag.starter/src/main/jastadd/RobotModel.relast +++ b/ros2rag.starter/src/main/jastadd/RobotModel.relast @@ -4,10 +4,10 @@ ZoneModel ::= SafetyZone:Zone* ; Zone ::= Coordinate* ; -RobotArm ::= Joint* EndEffector /<AppropriateSpeed:double>/ ; +RobotArm ::= Link* EndEffector /<AppropriateSpeed:double>/ ; -Joint ::= <Name:String> <CurrentPosition:IntPosition> ; +Link ::= <Name:String> <CurrentPosition:IntPosition> ; -EndEffector : Joint; +EndEffector : Link; Coordinate ::= <Position:IntPosition> ; diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java index 66a48f60b39cb7c5d48af9993eeca7159dc92e51..570b219e35c9f6defbe5d6041eece66a21af508e 100644 --- a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java @@ -1,15 +1,18 @@ package de.tudresden.inf.st.ros2rag.starter; import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.dataformat.yaml.YAMLFactory; import config.Dataconfig; import de.tudresden.inf.st.ros2rag.starter.ast.*; import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration; -import de.tudresden.inf.st.ros2rag.starter.data.DataJoint; +import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration.ActualConfiguration; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; import java.io.File; import java.io.IOException; +import java.util.Map.Entry; +import java.util.SortedMap; import java.util.concurrent.CountDownLatch; import java.util.concurrent.TimeUnit; @@ -25,26 +28,26 @@ public class StarterMain { private Model model; public void run(String[] args) throws IOException, InterruptedException { - File configFile = new File(args[0]); - - final int[][] safetyZoneCoordinates = { - {1, 1, 0}, - {-1, -1, 1} - }; + File configFile = new File(args.length == 0 ? "./src/main/resources/config.yaml" : args[0]); // --- No configuration below this line --- - ObjectMapper mapper = new ObjectMapper(); + ObjectMapper mapper = new ObjectMapper(new YAMLFactory()); System.out.println("Using config file: " + configFile.getAbsolutePath()); - DataConfiguration config = mapper.readValue(configFile, DataConfiguration.class); + ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector; model = new Model(); - model.MqttSetHost(config.mqttHost); + Util.setMqttHost(model::MqttSetHost, config); ZoneModel zoneModel = new ZoneModel(); Zone safetyZone = new Zone(); - for (int[] coordinate : safetyZoneCoordinates) { + for (String zone : config.zones) { + int[] coordinate = {0, 0, 0}; + String[] zoneSplit = zone.split(" "); + for (int i = 0; i < zoneSplit.length; i++) { + coordinate[i] = Integer.parseInt(zoneSplit[i]); + } safetyZone.addCoordinate(new Coordinate( IntPosition.of(coordinate[0], coordinate[1], coordinate[2]))); } @@ -55,24 +58,23 @@ public class StarterMain { RobotArm robotArm = new RobotArm(); model.setRobotArm(robotArm); - for (DataJoint dataJoint : config.joints) { - final Joint jointOrEndEffector; - if (dataJoint.isEndEffector) { + Util.iterateLinks((isEndEffector, topic, name) -> { + Link link; + if (isEndEffector) { EndEffector endEffector = new EndEffector(); robotArm.setEndEffector(endEffector); - jointOrEndEffector = endEffector; + link = endEffector; } else { - Joint joint = new Joint(); - robotArm.addJoint(joint); - jointOrEndEffector = joint; + link = new Link(); + robotArm.addLink(link); } - jointOrEndEffector.setName(dataJoint.name); - jointOrEndEffector.setCurrentPosition(makePosition(0, 0, 0)); - robotArm.addDependency1(jointOrEndEffector); - jointOrEndEffector.connectCurrentPosition(dataJoint.topic); - } + link.setName(name); + link.setCurrentPosition(makePosition(0, 0, 0)); + link.containingRobotArm().addDependency1(link); + link.connectCurrentPosition(topic); + }, config); - robotArm.connectAppropriateSpeed(config.robotConfigTopic, true); + robotArm.connectAppropriateSpeed(config.topics.robotConfig, true); logStatus("Start", robotArm); CountDownLatch exitCondition = new CountDownLatch(1); @@ -81,12 +83,12 @@ public class StarterMain { logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C."); mainHandler = new MqttUpdater("mainHandler"); - mainHandler.setHost(config.mqttHost); + Util.setMqttHost(mainHandler, config); mainHandler.waitUntilReady(2, TimeUnit.SECONDS); mainHandler.newConnection("exit", bytes -> exitCondition.countDown()); mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm)); - sendInitialDataConfig(mainHandler, config.dataConfigTopic); + sendInitialDataConfig(mainHandler, config.topics.dataConfig); Runtime.getRuntime().addShutdownHook(new Thread(this::close)); @@ -110,7 +112,7 @@ public class StarterMain { StringBuilder sb = new StringBuilder(prefix).append("\n") .append("robotArm.isInSafetyZone: ").append(robotArm.isInSafetyZone()) .append(", robotArm.getAppropriateSpeed = ").append(robotArm.getAppropriateSpeed()).append("\n"); - for (Joint joint : robotArm.getJointList()) { + for (Link joint : robotArm.getLinkList()) { sb.append(joint.getName()).append(": ").append(joint.getCurrentPosition()).append("\n"); } sb.append("endEffector ").append(robotArm.getEndEffector().getName()).append(": ") diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Util.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Util.java new file mode 100644 index 0000000000000000000000000000000000000000..22a27e5cfbfac0f1cec983238a34ffec14501aad --- /dev/null +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Util.java @@ -0,0 +1,67 @@ +package de.tudresden.inf.st.ros2rag.starter; + +import de.tudresden.inf.st.ros2rag.starter.ast.Link; +import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater; +import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration; +import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration.ActualConfiguration; + +import java.io.IOException; +import java.util.Map; +import java.util.SortedMap; + +/** + * Helper method dealing with config. + * + * @author rschoene - Initial contribution + */ +public class Util { + public static void setMqttHost(RootElement model, ActualConfiguration config) throws IOException { + HostAndPort hostAndPort = split(config.server); + model.MqttSetHost(hostAndPort.host, hostAndPort.port); + } + + public static void setMqttHost(MqttUpdater handler, ActualConfiguration config) throws IOException { + HostAndPort hostAndPort = split(config.server); + handler.setHost(hostAndPort.host, hostAndPort.port); + } + + public static void iterateLinks(HandleLink callback, ActualConfiguration config) { + 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); + } + } + 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; + } + + private static class HostAndPort { + String host; + int port; + } + + @FunctionalInterface + public interface RootElement { + void MqttSetHost(String host, int port) throws IOException; + } + + @FunctionalInterface + public interface HandleLink { + void handle(boolean isEndEffector, String topic, String name); + } +} diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java index b2ec3639dc485971c58a91b0cd4916b37e79c0b2..092d498d6fcb7535eb016d06efeffeb54a8b9a52 100644 --- a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java @@ -1,7 +1,9 @@ package de.tudresden.inf.st.ros2rag.starter.data; -import java.util.ArrayList; +import java.util.HashMap; import java.util.List; +import java.util.Map; +import java.util.SortedMap; /** * Data class for initial configuration. @@ -9,9 +11,17 @@ import java.util.List; * @author rschoene - Initial contribution */ public class DataConfiguration { - public List<DataJoint> joints = new ArrayList<>(); - public String robotConfigTopic; - public String dataConfigTopic; - public int exitAfterSeconds = 0; - public String mqttHost = "localhost"; + public ActualConfiguration panda_mqtt_connector; + public static class ActualConfiguration { + public String server = "tcp://localhost:1883"; + public DataTopics topics; + public int zone_size; + public List<String> zones; + public Map<String, SortedMap<String, String>> parts; + public Map<String, SortedMap<String, String>> end_effectors; + } + public static class DataTopics { + public String robotConfig; + public String dataConfig; + } } diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java deleted file mode 100644 index 2431827d30a9742f0366283c39d1dc156b876da2..0000000000000000000000000000000000000000 --- a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java +++ /dev/null @@ -1,12 +0,0 @@ -package de.tudresden.inf.st.ros2rag.starter.data; - -/** - * Data class to describe a joint. - * - * @author rschoene - Initial contribution - */ -public class DataJoint { - public String topic; - public String name; - public boolean isEndEffector = false; -} diff --git a/ros2rag.starter/src/main/resources/config.yaml b/ros2rag.starter/src/main/resources/config.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a18ba849e3714314ed726f9e2780839dc1cae3ae --- /dev/null +++ b/ros2rag.starter/src/main/resources/config.yaml @@ -0,0 +1,23 @@ +panda_mqtt_connector: + server: "tcp://localhost:1883" + topics: + robotConfig: "robotconfig" + dataConfig: "dataconfig" + zone_size: 0.5 + zones: + - "1 1" + - "-1 -1 1" + parts: + panda: + Link0: "panda::panda_link0" + Link1: "panda::panda_link1" + Link2: "panda::panda_link2" + Link3: "panda::panda_link3" + Link4: "panda::panda_link4" + Link5: "panda::panda_link5" + Link6: "panda::panda_link6" + LeftFinger: "panda::panda_leftfinger" + RightFinger: "panda::panda_rightfinger" + end_effectors: + panda: + EndEffector: "panda::panda_link7"