From 16a5550d0ec1a35a74fdfa6f0040b85db9e7876b Mon Sep 17 00:00:00 2001 From: rschoene <rene.schoene@tu-dresden.de> Date: Thu, 28 May 2020 10:59:11 +0200 Subject: [PATCH] Towards functional starter. - sender/receiver: reuse generated MqttUpdater - base: added option "--verbose" to print stacktrace if any - base: move mqtt default port definition to MqttUpdater - base: add option in MqttUpdater to suppress welcome message - starter: create 10 joints, use correct mqtt topics --- exit.sh | 2 + receiver.sh | 1 + .../src/main/jastadd/backend/Generation.jadd | 4 +- .../jastadd/ros2rag/compiler/Compiler.java | 19 +- .../src/main/resources/MqttUpdater.jadd | 61 +++-- .../src/main/jastadd/Example.relast | 13 - .../src/main/jastadd/Example.ros2rag | 22 -- .../src/main/jastadd/Generated.jrag | 137 ---------- .../src/main/jastadd/Generated.relast | 5 - .../inf/st/ros2rag/example/Main.java | 143 ----------- .../inf/st/ros2rag/example/MqttUpdater.java | 236 ------------------ ros2rag.receiverstub/build.gradle | 4 +- .../inf/st/ros2rag/receiverstub/Main.java | 3 +- ros2rag.senderstub/build.gradle | 4 +- .../inf/st/ros2rag/senderstub/Main.java | 49 ++-- .../.gitignore | 0 .../build.gradle | 93 ++++--- .../src/main/jastadd/Computation.jrag | 2 +- .../src/main/jastadd/Definitions.ros2rag | 30 +++ .../src/main/jastadd/Navigation.jrag | 0 .../src/main/jastadd/RobotModel.jadd | 0 .../src/main/jastadd/RobotModel.relast | 13 + .../inf/st/ros2rag/starter/Main.java | 125 ++++++++++ .../src/main/proto/dataconfig.proto | 0 .../src/main/proto/linkstate.proto | 0 .../src/main/proto/robotconfig.proto | 0 .../src/main/resources/log4j2.xml | 0 settings.gradle | 2 +- starter.sh | 3 + 29 files changed, 326 insertions(+), 645 deletions(-) create mode 100755 exit.sh delete mode 100644 ros2rag.example/src/main/jastadd/Example.relast delete mode 100644 ros2rag.example/src/main/jastadd/Example.ros2rag delete mode 100644 ros2rag.example/src/main/jastadd/Generated.jrag delete mode 100644 ros2rag.example/src/main/jastadd/Generated.relast delete mode 100644 ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/Main.java delete mode 100644 ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/MqttUpdater.java rename {ros2rag.example => ros2rag.starter}/.gitignore (100%) rename {ros2rag.example => ros2rag.starter}/build.gradle (75%) rename {ros2rag.example => ros2rag.starter}/src/main/jastadd/Computation.jrag (95%) create mode 100644 ros2rag.starter/src/main/jastadd/Definitions.ros2rag rename {ros2rag.example => ros2rag.starter}/src/main/jastadd/Navigation.jrag (100%) rename ros2rag.example/src/main/jastadd/Example.jadd => ros2rag.starter/src/main/jastadd/RobotModel.jadd (100%) create mode 100644 ros2rag.starter/src/main/jastadd/RobotModel.relast create mode 100644 ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java rename {ros2rag.example => ros2rag.starter}/src/main/proto/dataconfig.proto (100%) rename {ros2rag.example => ros2rag.starter}/src/main/proto/linkstate.proto (100%) rename {ros2rag.example => ros2rag.starter}/src/main/proto/robotconfig.proto (100%) rename {ros2rag.example => ros2rag.starter}/src/main/resources/log4j2.xml (100%) create mode 100755 starter.sh diff --git a/exit.sh b/exit.sh new file mode 100755 index 0000000..92011c0 --- /dev/null +++ b/exit.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +./ros2rag.senderstub/build/install/ros2rag.senderstub/bin/ros2rag.senderstub exit 1 diff --git a/receiver.sh b/receiver.sh index 732ebc2..a242dc3 100755 --- a/receiver.sh +++ b/receiver.sh @@ -1,2 +1,3 @@ #!/usr/bin/env bash +./gradlew :ros2rag.receiverstub:installDist ./ros2rag.receiverstub/build/install/ros2rag.receiverstub/bin/ros2rag.receiverstub $@ diff --git a/ros2rag.base/src/main/jastadd/backend/Generation.jadd b/ros2rag.base/src/main/jastadd/backend/Generation.jadd index 75be20c..b74743d 100644 --- a/ros2rag.base/src/main/jastadd/backend/Generation.jadd +++ b/ros2rag.base/src/main/jastadd/backend/Generation.jadd @@ -50,15 +50,13 @@ aspect AspectGeneration { public void Ros2Rag.generateMqttAspect(StringBuilder sb, TypeDecl rootNode) { String rootNodeName = rootNode.getName(); sb.append("aspect MQTT {\n"); - sb.append(ind(1)).append("private static final int ") - .append(rootNodeName).append("._MQTT_DEFAULT_PORT = 1883;\n"); sb.append(ind(1)).append("private MqttUpdater ").append(rootNodeName) .append(".").append(mqttUpdaterField()).append(" = new MqttUpdater();\n"); // mqttSetHost(String host) sb.append(ind(1)).append("public void ").append(rootNodeName).append(".") .append(mqttSetHostMethod()).append("(String host) throws java.io.IOException {\n"); - sb.append(ind(2)).append("MqttSetHost(host, _MQTT_DEFAULT_PORT);\n"); + sb.append(ind(2)).append(mqttUpdaterField()).append(".setHost(host);\n"); sb.append(ind(1)).append("}\n"); // mqttSetHost(String host, int port) diff --git a/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/Compiler.java b/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/Compiler.java index 7cd97a3..af1076e 100644 --- a/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/Compiler.java +++ b/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/Compiler.java @@ -24,6 +24,7 @@ public class Compiler { private StringOption optionRootNode; private StringOption optionInputRos2Rag; private FlagOption optionHelp; + private FlagOption optionVerbose; private ArrayList<Option<?>> options; private CommandLine commandLine; @@ -44,6 +45,19 @@ public class Compiler { return; } + if (optionVerbose.isSet()) { + try { + run(); + } catch (CompilerException e) { + e.printStackTrace(); + throw e; + } + } else { + run(); + } + } + + private void run() throws CompilerException { String outputDir; if (optionOutputDir.isSet()) { outputDir = optionOutputDir.getValue(); @@ -129,6 +143,7 @@ public class Compiler { optionRootNode = addOption(new StringOption("rootNode", "root node in the base grammar.")); optionInputRos2Rag = addOption(new StringOption("inputRos2Rag", "ros2rag definition file.")); optionHelp = addOption(new FlagOption("help", "Print usage and exit.")); + optionVerbose = addOption(new FlagOption("verbose", "Print more messages.")); } private <OptionType extends Option<?>> OptionType addOption(OptionType option) { @@ -145,7 +160,9 @@ public class Compiler { Ros2RagScanner scanner = new Ros2RagScanner(reader); Ros2RagParser parser = new Ros2RagParser(); inputGrammar = (GrammarFile) parser.parse(scanner); - inputGrammar.dumpTree(System.out); + if (optionVerbose.isSet()) { + inputGrammar.dumpTree(System.out); + } program.addGrammarFile(inputGrammar); inputGrammar.treeResolveAll(); } catch (IOException | Parser.Exception e) { diff --git a/ros2rag.base/src/main/resources/MqttUpdater.jadd b/ros2rag.base/src/main/resources/MqttUpdater.jadd index 6e986f2..394735c 100644 --- a/ros2rag.base/src/main/resources/MqttUpdater.jadd +++ b/ros2rag.base/src/main/resources/MqttUpdater.jadd @@ -1,9 +1,11 @@ +aspect MqttUpdater { /** * Helper class to receive updates via MQTT and use callbacks to handle those messages. * * @author rschoene - Initial contribution */ public class MqttUpdater { + private static final int DEFAULT_PORT = 1883; private final org.apache.logging.log4j.Logger logger; private final String name; @@ -16,6 +18,7 @@ public class MqttUpdater { private final java.util.concurrent.locks.Condition readyCondition; private final java.util.concurrent.locks.Lock readyLock; private boolean ready; + private boolean sendWelcomeMessage = true; private org.fusesource.mqtt.client.QoS qos; /** Dispatch knowledge */ private final java.util.Map<String, java.util.function.Consumer<byte[]>> callbacks; @@ -34,16 +37,31 @@ public class MqttUpdater { this.qos = org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE; } + public MqttUpdater dontSendWelcomeMessage() { + this.sendWelcomeMessage = false; + return this; + } + + /** + * Sets the host (with default port) to receive messages from, and connects to it. + * @throws IOException if could not connect, or could not subscribe to a topic + * @return self + */ + public MqttUpdater setHost(String host) throws java.io.IOException { + return setHost(host, DEFAULT_PORT); + } + /** * Sets the host to receive messages from, and connects to it. * @throws IOException if could not connect, or could not subscribe to a topic * @return self */ public MqttUpdater setHost(String host, int port) throws java.io.IOException { + java.util.Objects.requireNonNull(host, "Host need to be set!"); + this.host = java.net.URI.create("tcp://" + host + ":" + port); logger.debug("Host for {} is {}", this.name, this.host); - java.util.Objects.requireNonNull(this.host, "Host need to be set!"); org.fusesource.mqtt.client.MQTT mqtt = new org.fusesource.mqtt.client.MQTT(); mqtt.setHost(this.host); connection = mqtt.callbackConnection(); @@ -91,24 +109,22 @@ public class MqttUpdater { connection.connect(new org.fusesource.mqtt.client.Callback<Void>() { @Override public void onSuccess(Void value) { - connection.publish("components", (name + " is connected").getBytes(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false, new org.fusesource.mqtt.client.Callback<Void>() { - @Override - public void onSuccess(Void value) { - logger.debug("success sending welcome message"); - try { - readyLock.lock(); - ready = true; - readyCondition.signalAll(); - } finally { - readyLock.unlock(); + if (MqttUpdater.this.sendWelcomeMessage) { + connection.publish("components", (name + " is connected").getBytes(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false, new org.fusesource.mqtt.client.Callback<Void>() { + @Override + public void onSuccess(Void value) { + logger.debug("success sending welcome message"); + setReady(); } - } - @Override - public void onFailure(Throwable value) { - logger.debug("failure sending welcome message", value); - } - }); + @Override + public void onFailure(Throwable value) { + logger.debug("failure sending welcome message", value); + } + }); + } else { + setReady(); + } } @Override @@ -125,6 +141,16 @@ public class MqttUpdater { return host; } + private void setReady() { + try { + readyLock.lock(); + ready = true; + readyCondition.signalAll(); + } finally { + readyLock.unlock(); + } + } + private void throwIf(java.util.concurrent.atomic.AtomicReference<Throwable> error) throws java.io.IOException { if (error.get() != null) { throw new java.io.IOException(error.get()); @@ -220,3 +246,4 @@ public class MqttUpdater { }); } } +} diff --git a/ros2rag.example/src/main/jastadd/Example.relast b/ros2rag.example/src/main/jastadd/Example.relast deleted file mode 100644 index f4b2aa0..0000000 --- a/ros2rag.example/src/main/jastadd/Example.relast +++ /dev/null @@ -1,13 +0,0 @@ -Model ::= RobotArm ZoneModel ; - -ZoneModel ::= <Size:IntPosition> SafetyZone:Zone* ; - -Zone ::= Coordinate* ; - -RobotArm ::= Joint* EndEffector <_AttributeTestSource:int> /<_AppropriateSpeed:double>/ ; // normally this would be: <AttributeTestSource:int> ; - -Joint ::= <Name> <_CurrentPosition:IntPosition> ; // normally this would be: <CurrentPosition:IntPosition> - -EndEffector : Joint; - -Coordinate ::= <Position:IntPosition> ; diff --git a/ros2rag.example/src/main/jastadd/Example.ros2rag b/ros2rag.example/src/main/jastadd/Example.ros2rag deleted file mode 100644 index e65c535..0000000 --- a/ros2rag.example/src/main/jastadd/Example.ros2rag +++ /dev/null @@ -1,22 +0,0 @@ -/** - * Version 2020-04-17 - */ -// --- update definitions --- -read Joint.CurrentPosition using LinkStateToIntPosition ; -write RobotArm._AppropriateSpeed using CreateSpeedMessage ; - -// --- dependency definitions --- -RobotArm._AppropriateSpeed canDependOn Joint.CurrentPosition as dependency1 ; -RobotArm._AppropriateSpeed canDependOn RobotArm._AttributeTestSource as dependency2 ; - -// --- mapping definitions --- -LinkStateToIntPosition maps protobuf panda.Linkstate.PandaLinkState x to IntPosition y { - panda.Linkstate.PandaLinkState.Position p = x.getPos(); - y = IntPosition.of((int) p.getPositionX(), (int) p.getPositionY(), (int) p.getPositionZ()); -} - -CreateSpeedMessage maps double x to protobuf config.Robotconfig.RobotConfig y { - y = config.Robotconfig.RobotConfig.newBuilder() - .setSpeed(x) - .build(); -} diff --git a/ros2rag.example/src/main/jastadd/Generated.jrag b/ros2rag.example/src/main/jastadd/Generated.jrag deleted file mode 100644 index 2dfef1f..0000000 --- a/ros2rag.example/src/main/jastadd/Generated.jrag +++ /dev/null @@ -1,137 +0,0 @@ -import de.tudresden.inf.st.ros2rag.example.MqttUpdater; -import panda.Linkstate.PandaLinkState.Position; - -// this aspect depends on the actual grammar. probably we need to provide the root node type, in this case "Model" -// it is somewhat problematic, because it assumes a single root to store the mqtt-host -aspect MQTT { - private MqttUpdater Model._mqttUpdater = new MqttUpdater(); - private static final int Model._MQTT_DEFAULT_PORT = 1883; - - public void Model.MqttSetHost(String host) throws java.io.IOException { - MqttSetHost(host, _MQTT_DEFAULT_PORT); - } - - public void Model.MqttSetHost(String host, int port) throws java.io.IOException { - _mqttUpdater.setHost(host, port); - } - - public boolean Model.MqttWaitUntilReady(long time, java.util.concurrent.TimeUnit unit) { - return _mqttUpdater.waitUntilReady(time, unit); - } - - public void Model.MqttCloseConnections() { - _mqttUpdater.close(); - } - - inh MqttUpdater Joint._mqttUpdater(); - inh MqttUpdater RobotArm._mqttUpdater(); - eq Model.getRobotArm()._mqttUpdater() = _mqttUpdater; - eq Model.getZoneModel()._mqttUpdater() = _mqttUpdater; -} - -aspect GrammarExtension { - // --- Joint --- - public void Joint.connectCurrentPosition(String topic) { - _mqttUpdater().newConnection(topic, message -> { - // Parse message into a PandaLinkState - try { - System.out.println("begin update current position"); - panda.Linkstate.PandaLinkState x = panda.Linkstate.PandaLinkState.parseFrom(message); - panda.Linkstate.PandaLinkState.Position p = x.getPos(); - IntPosition y = IntPosition.of((int) p.getPositionX(), (int) p.getPositionY(), (int) p.getPositionZ()); - // >> when "always" is absent, generate this - if (getCurrentPosition().equals(y)) { return; } - // >> until here - System.out.println("really setting current position"); - setCurrentPosition(y); - } catch (com.google.protobuf.InvalidProtocolBufferException e) { - e.printStackTrace(); - } - }); - } - - public Joint Joint.setCurrentPosition(IntPosition value) { - set_CurrentPosition(value); - for (RobotArm target : get_dependency1Targets()) { - System.out.println("checking robotarm " + target); - if (target._update_AppropriateSpeed()) { - System.out.println("writing " + target + "_AppropriateSpeed"); - target._write_AppropriateSpeed_lastValue(); - } - } - return this; - } - - public IntPosition Joint.getCurrentPosition() { - return get_CurrentPosition(); - } - - // --- RobotArm --- - /* - this is one way to store the topic, i.e., close to the producing nonterminal. - another way would be a map (or multiple) in the MqttUpdater mapping nonterminal and operation to a topic - */ - private String RobotArm._write_AppropriateSpeed_topic = null; - private config.Robotconfig.RobotConfig RobotArm._write_AppropriateSpeed_lastValue = null; - - public RobotArm RobotArm.setAttributeTestSource(int value) { - set_AttributeTestSource(value); - for (RobotArm target : get_dependency2Targets()) { - if (target._update_AppropriateSpeed()) { - target._write_AppropriateSpeed_lastValue(); - } - } - return this; - } - - public int RobotArm.getAttributeTestSource() { - return get_AttributeTestSource(); - } - - public void RobotArm.connect_AppropriateSpeed(String topic, boolean writeCurrentValue) { - _write_AppropriateSpeed_topic = topic; - _update_AppropriateSpeed(); - if (writeCurrentValue) { - _write_AppropriateSpeed_lastValue(); - } - } - - /** - * @apilevel internal - * @return true, if CreateSpeedMessage changed for _AppropriateSpeed - */ - public boolean RobotArm._update_AppropriateSpeed() { - // 1) read _AppropriateSpeed (which triggers computation, right?) - // 2) maybe: check if value has changed since last publish - get_AppropriateSpeed_reset(); - double x = get_AppropriateSpeed(); - // begin transformation "CreateSpeedMessage" - config.Robotconfig.RobotConfig y = config.Robotconfig.RobotConfig.newBuilder() - .setSpeed(x) - .build(); - // end transformation "CreateSpeedMessage" - if (y.equals(_write_AppropriateSpeed_lastValue)) { - // if always is absent, then return here - return false; - } - // remember last value - _write_AppropriateSpeed_lastValue = y; - return true; - } - - public void RobotArm._write_AppropriateSpeed_lastValue() { - // 3) publish - // we know, it is a protobuf type, so call "toByteArray" - byte[] bytes = _write_AppropriateSpeed_lastValue.toByteArray(); - _mqttUpdater().publish(_write_AppropriateSpeed_topic, bytes); - } - - public void RobotArm.addDependency1(Joint source) { - this.add_dependency1Source(source); - } - - public void RobotArm.addDependency2(RobotArm source) { - this.add_dependency2Source(source); - } - -} diff --git a/ros2rag.example/src/main/jastadd/Generated.relast b/ros2rag.example/src/main/jastadd/Generated.relast deleted file mode 100644 index af903c2..0000000 --- a/ros2rag.example/src/main/jastadd/Generated.relast +++ /dev/null @@ -1,5 +0,0 @@ -// RobotArm._AppropriateSpeed canDependOn Joint.CurrentPosition ; -rel RobotArm._dependency1Source* <-> Joint._dependency1Target* ; - -// RobotArm._AppropriateSpeed canDependOn RobotArm._AttributeTestSource as dependency2 ; -rel RobotArm._dependency2Source* <-> RobotArm._dependency2Target* ; diff --git a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/Main.java b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/Main.java deleted file mode 100644 index 8529fa2..0000000 --- a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/Main.java +++ /dev/null @@ -1,143 +0,0 @@ -package de.tudresden.inf.st.ros2rag.example; - -import de.tudresden.inf.st.ros2rag.ast.*; -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; -import panda.Linkstate; - -import java.io.IOException; -import java.util.concurrent.TimeUnit; - -/** - * Testing Ros2Rag without generating something. - * - * @author rschoene - Initial contribution - */ -public class Main { - - private static final String TOPIC_JOINT1 = "robot/joint1"; - private static final String TOPIC_CONFIG = "robot/config"; - - private static final Logger logger = LogManager.getLogger(Main.class); - - public static void main(String[] args) throws IOException, InterruptedException { - Model model = new Model(); - model.MqttSetHost("localhost"); - - ZoneModel zoneModel = new ZoneModel(); - zoneModel.setSize(makePosition(1, 1, 1)); - - IntPosition myPosition = makePosition(0, 0, 0); - Coordinate myCoordinate = new Coordinate(myPosition); - Coordinate leftPosition = new Coordinate(makePosition(-1, 0, 0)); - Coordinate rightPosition = new Coordinate(makePosition(1, 0, 0)); - - Zone safetyZone = new Zone(); - safetyZone.addCoordinate(myCoordinate); - safetyZone.addCoordinate(leftPosition); - safetyZone.addCoordinate(rightPosition); - zoneModel.addSafetyZone(safetyZone); - model.setZoneModel(zoneModel); - - RobotArm robotArm = new RobotArm(); - robotArm.set_AttributeTestSource(1); // set initial value, no trigger - - Joint joint1 = new Joint(); - joint1.setName("joint1"); - joint1.setCurrentPosition(myPosition); - - EndEffector endEffector = new EndEffector(); - endEffector.setName("gripper"); - endEffector.setCurrentPosition(makePosition(2, 2, 3)); - - robotArm.addJoint(joint1); - robotArm.setEndEffector(endEffector); - model.setRobotArm(robotArm); - - // add dependencies - robotArm.addDependency1(joint1); - robotArm.addDependency1(endEffector); - robotArm.addDependency2(robotArm); - - model.MqttWaitUntilReady(2, TimeUnit.SECONDS); - - joint1.connectCurrentPosition(TOPIC_JOINT1); - robotArm.connect_AppropriateSpeed(TOPIC_CONFIG, true); - - startDelayedSendOne(); - - logStatus("BEFORE", robotArm, joint1); -// logger.info("Now invoke ./send_one.sh"); - -// Thread.sleep(3000); -// robotArm.setAttributeTestSource(1); -// Thread.sleep(3000); -// robotArm.setAttributeTestSource(5); - Thread.sleep(6000); - - logStatus("AFTER", robotArm, joint1); - - logger.info("Now changing AttributeTestSource - this should not emit any new mqtt message"); - robotArm.setAttributeTestSource(1); - Thread.sleep(1000); - robotArm.setAttributeTestSource(5); - - model.MqttCloseConnections(); - } - - private static void logStatus(String prefix, RobotArm robotArm, Joint joint1) { - logger.info("{} joint1.getCurrentPosition() = {}, " + - "robotArm.isInSafetyZone = {}, " + - "robotArm.get_AppropriateSpeed = {}", - prefix, - joint1.getCurrentPosition(), - robotArm.isInSafetyZone(), - robotArm.get_AppropriateSpeed()); - } - - private static void startDelayedSendOne() { - final int millis = 1500; - new Thread(() -> { - try { - Linkstate.PandaLinkState pls = Linkstate.PandaLinkState.newBuilder() - .setName("Joint1") - .setPos(Linkstate.PandaLinkState.Position.newBuilder() - .setPositionX(0.5f) - .setPositionY(0.5f) - .setPositionZ(0.5f) - .build()) - .build(); - byte[] bytes = pls.toByteArray(); - MqttUpdater mqttUpdater = new MqttUpdater("sender").setHost("localhost", 1883); - - // wait until connected, but in total millis - long before = System.currentTimeMillis(); - mqttUpdater.waitUntilReady(millis, TimeUnit.MILLISECONDS); - long alreadyWaited = System.currentTimeMillis() - before; - Thread.sleep((long) millis - alreadyWaited); - - mqttUpdater.publish(TOPIC_JOINT1, bytes); - - Thread.sleep(1000); - pls = Linkstate.PandaLinkState.newBuilder() - .setName("Joint1") - .setPos(Linkstate.PandaLinkState.Position.newBuilder() - .setPositionX(2.5f) - .setPositionY(2.5f) - .setPositionZ(2.5f) - .build()) - .build(); - bytes = pls.toByteArray(); - mqttUpdater.publish(TOPIC_JOINT1, bytes); - - mqttUpdater.close(); - } catch (IOException | InterruptedException e) { - e.printStackTrace(); - } - }).start(); - } - - private static IntPosition makePosition(int x, int y, int z) { - return IntPosition.of(x, y, z); - } -} diff --git a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/MqttUpdater.java b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/MqttUpdater.java deleted file mode 100644 index 293ced5..0000000 --- a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/MqttUpdater.java +++ /dev/null @@ -1,236 +0,0 @@ -package de.tudresden.inf.st.ros2rag.example; - -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; -import org.fusesource.hawtbuf.Buffer; -import org.fusesource.hawtbuf.UTF8Buffer; -import org.fusesource.mqtt.client.*; - -import java.io.IOException; -import java.net.URI; -import java.util.HashMap; -import java.util.Map; -import java.util.Objects; -import java.util.concurrent.TimeUnit; -import java.util.concurrent.atomic.AtomicReference; -import java.util.concurrent.locks.Condition; -import java.util.concurrent.locks.Lock; -import java.util.concurrent.locks.ReentrantLock; -import java.util.function.Consumer; - -/** - * Helper class to receive updates via MQTT and use callbacks to handle those messages. - * - * @author rschoene - Initial contribution - */ -public class MqttUpdater { - - private final Logger logger; - private final String name; - - /** The host running the MQTT broker. */ - private URI host; - /** The connection to the MQTT broker. */ - private CallbackConnection connection; - /** Whether we are subscribed to the topics yet */ - private final Condition readyCondition; - private final Lock readyLock; - private boolean ready; - private QoS qos; - /** Dispatch knowledge */ - private final Map<String, Consumer<byte[]>> callbacks; - - public MqttUpdater() { - this("Ros2Rag"); - } - - public MqttUpdater(String name) { - this.name = Objects.requireNonNull(name, "Name must be set"); - this.logger = LogManager.getLogger(MqttUpdater.class); - this.callbacks = new HashMap<>(); - this.readyLock = new ReentrantLock(); - this.readyCondition = readyLock.newCondition(); - this.ready = false; - this.qos = QoS.AT_LEAST_ONCE; - } - - /** - * Sets the host to receive messages from, and connects to it. - * @throws IOException if could not connect, or could not subscribe to a topic - * @return self - */ - public MqttUpdater setHost(String host, int port) throws IOException { - this.host = URI.create("tcp://" + host + ":" + port); - logger.debug("Host for {} is {}", this.name, this.host); - - Objects.requireNonNull(this.host, "Host need to be set!"); - MQTT mqtt = new MQTT(); - mqtt.setHost(this.host); - connection = mqtt.callbackConnection(); - AtomicReference<Throwable> error = new AtomicReference<>(); - - // add the listener to dispatch messages later - connection.listener(new ExtendedListener() { - public void onConnected() { - logger.debug("Connected"); - } - - @Override - public void onDisconnected() { - logger.debug("Disconnected"); - } - - @Override - public void onPublish(UTF8Buffer topic, Buffer body, Callback<Callback<Void>> ack) { - String topicString = topic.toString(); - Consumer<byte[]> callback = callbacks.get(topicString); - if (callback == null) { - logger.debug("Got a message, but no callback to call. Forgot to unsubscribe?"); - } else { - byte[] message = body.toByteArray(); -// System.out.println("message = " + Arrays.toString(message)); - callback.accept(message); - } - ack.onSuccess(null); // always acknowledge message - } - - @Override - public void onPublish(UTF8Buffer topicBuffer, Buffer body, Runnable ack) { - logger.warn("onPublish should not be called"); - } - - @Override - public void onFailure(Throwable cause) { -// logger.catching(cause); - error.set(cause); - } - }); - throwIf(error); - - // actually establish the connection - connection.connect(new Callback<Void>() { - @Override - public void onSuccess(Void value) { - connection.publish("components", (name + " is connected").getBytes(), QoS.AT_LEAST_ONCE, false, new Callback<Void>() { - @Override - public void onSuccess(Void value) { - logger.debug("success sending welcome message"); - try { - readyLock.lock(); - ready = true; - readyCondition.signalAll(); - } finally { - readyLock.unlock(); - } - } - - @Override - public void onFailure(Throwable value) { - logger.debug("failure sending welcome message", value); - } - }); - } - - @Override - public void onFailure(Throwable cause) { -// logger.error("Could not connect", cause); - error.set(cause); - } - }); - throwIf(error); - return this; - } - - public URI getHost() { - return host; - } - - private void throwIf(AtomicReference<Throwable> error) throws IOException { - if (error.get() != null) { - throw new IOException(error.get()); - } - } - - public void setQoSForSubscription(QoS qos) { - this.qos = qos; - } - - public void newConnection(String topic, Consumer<byte[]> callback) { - if (!ready) { - // TODO should maybe be something more kind than throwing an exception here - throw new IllegalStateException("Updater not ready"); - } - // register callback - callbacks.put(topic, callback); - - // subscribe at broker - Topic[] topicArray = { new Topic(topic, this.qos) }; - connection.subscribe(topicArray, new Callback<byte[]>() { - @Override - public void onSuccess(byte[] qoses) { - logger.debug("Subscribed to {}, qoses: {}", topic, qoses); - } - - @Override - public void onFailure(Throwable cause) { - logger.error("Could not subscribe to {}", topic, cause); - } - }); - } - - /** - * Waits until this updater is ready to receive MQTT messages. - * If it already is ready, return immediately with the value <code>true</code>. - * Otherwise waits for the given amount of time, and either return <code>true</code> within the timespan, - * if it got ready, or <code>false</code> upon a timeout. - * @param time the maximum time to wait - * @param unit the time unit of the time argument - * @return whether this updater is ready - */ - public boolean waitUntilReady(long time, TimeUnit unit) { - try { - readyLock.lock(); - if (ready) { - return true; - } - return readyCondition.await(time, unit); - } catch (InterruptedException e) { - e.printStackTrace(); - } finally { - readyLock.unlock(); - } - return false; - } - - public void close() { - if (connection == null) { - logger.warn("Stopping without connection. Was setHost() called?"); - return; - } - connection.disconnect(new Callback<Void>() { - @Override - public void onSuccess(Void value) { - logger.info("Disconnected {} from {}", name, host); - } - - @Override - public void onFailure(Throwable ignored) { - // Disconnects never fail. And we do not care either. - } - }); - } - - public void publish(String topic, byte[] bytes) { - connection.publish(topic, bytes, qos, false, new Callback<Void>() { - @Override - public void onSuccess(Void value) { - logger.debug("Published some bytes to {}", topic); - } - - @Override - public void onFailure(Throwable value) { - logger.warn("Could not publish on topic '{}'", topic); - } - }); - } -} diff --git a/ros2rag.receiverstub/build.gradle b/ros2rag.receiverstub/build.gradle index 815b43e..0fe6ce1 100644 --- a/ros2rag.receiverstub/build.gradle +++ b/ros2rag.receiverstub/build.gradle @@ -20,14 +20,14 @@ sourceSets.main.java.srcDir "src/gen/java" jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.receiverstub.Main') dependencies { + implementation project(':ros2rag.starter') + 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: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11' compile 'com.google.protobuf:protobuf-java:3.0.0' compile group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15' - implementation project(':ros2rag.example') - protobuf files("$projectDir/../ros2rag.example/src/main/proto") } 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 36a0334..a744181 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 @@ -2,12 +2,11 @@ package de.tudresden.inf.st.ros2rag.receiverstub; import com.google.protobuf.InvalidProtocolBufferException; import config.Robotconfig.RobotConfig; -import de.tudresden.inf.st.ros2rag.example.MqttUpdater; +import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater; import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; import panda.Linkstate.PandaLinkState; -import java.text.MessageFormat; import java.util.concurrent.TimeUnit; import java.util.concurrent.locks.Condition; import java.util.concurrent.locks.Lock; diff --git a/ros2rag.senderstub/build.gradle b/ros2rag.senderstub/build.gradle index 517e991..362a729 100644 --- a/ros2rag.senderstub/build.gradle +++ b/ros2rag.senderstub/build.gradle @@ -20,14 +20,14 @@ sourceSets.main.java.srcDir "src/gen/java" jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.senderstub.Main') dependencies { + implementation project(':ros2rag.starter') + 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: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11' compile 'com.google.protobuf:protobuf-java:3.0.0' compile group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15' - implementation project(':ros2rag.example') - protobuf files("$projectDir/../ros2rag.example/src/main/proto") } diff --git a/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/Main.java b/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/Main.java index 8a3fd0a..a267836 100644 --- a/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/Main.java +++ b/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/Main.java @@ -1,34 +1,45 @@ package de.tudresden.inf.st.ros2rag.senderstub; -import de.tudresden.inf.st.ros2rag.example.MqttUpdater; +import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater; import panda.Linkstate; +import java.util.concurrent.TimeUnit; + public class Main { public static void main(String[] args) throws Exception { - String topic; + final String topic; + final byte[] message; + if (args.length < 1) { topic = "robot/joint1"; } else { topic = args[0]; } - Linkstate.PandaLinkState pls = Linkstate.PandaLinkState.newBuilder() - .setName("Joint1") - .setPos(Linkstate.PandaLinkState.Position.newBuilder() - .setPositionX(0.5f) - .setPositionY(0.5f) - .setPositionZ(0.5f) - .build()) - .setOrient(Linkstate.PandaLinkState.Orientation.newBuilder() - .setOrientationX(0) - .setOrientationY(0) - .setOrientationZ(0) - .setOrientationW(0) - .build()) - .build(); - MqttUpdater sender = new MqttUpdater("sender stub"); + + if (args.length < 2) { + Linkstate.PandaLinkState pls = Linkstate.PandaLinkState.newBuilder() + .setName("Joint1") + .setPos(Linkstate.PandaLinkState.Position.newBuilder() + .setPositionX(0.5f) + .setPositionY(0.5f) + .setPositionZ(0.5f) + .build()) + .setOrient(Linkstate.PandaLinkState.Orientation.newBuilder() + .setOrientationX(0) + .setOrientationY(0) + .setOrientationZ(0) + .setOrientationW(0) + .build()) + .build(); + message = pls.toByteArray(); + } else { + message = args[1].getBytes(); + } + + MqttUpdater sender = new MqttUpdater("sender stub").dontSendWelcomeMessage(); sender.setHost("localhost", 1883); -// System.out.println("pls.toByteArray() = " + Arrays.toString(pls.toByteArray())); - sender.publish(topic, pls.toByteArray()); + sender.waitUntilReady(2, TimeUnit.SECONDS); + sender.publish(topic, message); sender.close(); } } diff --git a/ros2rag.example/.gitignore b/ros2rag.starter/.gitignore similarity index 100% rename from ros2rag.example/.gitignore rename to ros2rag.starter/.gitignore diff --git a/ros2rag.example/build.gradle b/ros2rag.starter/build.gradle similarity index 75% rename from ros2rag.example/build.gradle rename to ros2rag.starter/build.gradle index 56316f0..3bf47eb 100644 --- a/ros2rag.example/build.gradle +++ b/ros2rag.starter/build.gradle @@ -4,7 +4,7 @@ apply plugin: 'com.google.protobuf' sourceCompatibility = 1.8 -mainClassName = 'de.tudresden.inf.st.ros2rag.example.Main' +mainClassName = 'de.tudresden.inf.st.ros2rag.starter.Main' repositories { jcenter() @@ -18,10 +18,16 @@ buildscript { } } +configurations { + baseRuntimeClasspath +} + sourceSets.main.java.srcDir "src/gen/java" -jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.example.Main') +jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.starter.Main') dependencies { + implementation project (':ros2rag.base') + 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: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11' @@ -37,11 +43,51 @@ test { maxHeapSize = '1G' } +// Input files for relast +def relastFiles = ["src/gen/jastadd/Grammar.relast"] + +// phases: Ros2Rag -> RelAst -> JastAdd +// phase: Ros2Rag +task ros2rag(type: JavaExec) { + classpath = configurations.baseRuntimeClasspath + + group = 'Build' + main = 'org.jastadd.ros2rag.compiler.Compiler' + + args([ + '--verbose', + '--outputDir=src/gen/jastadd', + '--inputGrammar=src/main/jastadd/RobotModel.relast', + '--inputRos2Rag=src/main/jastadd/Definitions.ros2rag', + '--rootNode=Model' + ]) +} + +// phase: RelAst +task relastToJastAdd(type: JavaExec) { + group = 'Build' + main = "-jar" + + args(["../libs/relast.jar", + "--grammarName=./src/gen/jastadd/model", + "--useJastAddNames", + "--listClass=java.util.ArrayList", + "--jastAddList=JastAddList", + "--resolverHelper", + "--file"] + + + relastFiles) + + inputs.files relastFiles + outputs.files file("./src/gen/jastadd/model.ast"), file("./src/gen/jastadd/model.jadd") +} + +// phase: JastAdd jastadd { configureModuleBuild() modules { //noinspection GroovyAssignabilityCheck - module("ros2rag example") { + module("ros2rag starter") { java { basedir "src/" @@ -58,15 +104,6 @@ jastadd { include "gen/jastadd/**/*.jadd" include "gen/jastadd/**/*.jrag" } - - // scanner { - // include "src/main/jastadd/RelAst.flex" - // } - - // parser { - // include "src/main/jastadd/Preamble.parser" - // include "src/main/jastadd/RelAst.parser" - // } } } @@ -81,48 +118,22 @@ jastadd { } - module = "ros2rag example" + module = "ros2rag starter" - astPackage = 'de.tudresden.inf.st.ros2rag.ast' - - // parser.name = 'RelAstParser' + astPackage = 'de.tudresden.inf.st.ros2rag.starter.ast' genDir = 'src/gen/java' buildInfoDir = 'src/gen-res' - // scanner.genDir = "src/gen/java/org/jastadd/ros2rag/scanner" - // parser.genDir = "src/gen/java/org/jastadd/ros2rag/parser" - // jastaddOptions = ["--lineColumnNumbers", "--visitCheck=true", "--rewrite=cnta", "--cache=all"] // default options are: '--rewrite=cnta', '--safeLazy', '--visitCheck=false', '--cacheCycle=false' extraJastAddOptions = ["--lineColumnNumbers", '--List=JastAddList'] } -// Input files -def relastFiles = ["src/main/jastadd/Example.relast", "src/main/jastadd/Generated.relast"] - -// phase: RelAst -> JastAdd -task relastToJastAdd(type: JavaExec) { - group = 'Build' - main = "-jar" - - args(["../libs/relast.jar", - "--grammarName=./src/gen/jastadd/model", - "--useJastAddNames", - "--listClass=java.util.ArrayList", - "--jastAddList=JastAddList", - "--resolverHelper", - "--file"] - + - relastFiles) - - inputs.files relastFiles - outputs.files file("./src/gen/jastadd/model.ast"), file("./src/gen/jastadd/model.jadd") -} - // Workflow configuration for phases generateAst.dependsOn relastToJastAdd +relastToJastAdd.dependsOn ros2rag protobuf { // create strange directories, so use default here diff --git a/ros2rag.example/src/main/jastadd/Computation.jrag b/ros2rag.starter/src/main/jastadd/Computation.jrag similarity index 95% rename from ros2rag.example/src/main/jastadd/Computation.jrag rename to ros2rag.starter/src/main/jastadd/Computation.jrag index 4d72197..09c1512 100644 --- a/ros2rag.example/src/main/jastadd/Computation.jrag +++ b/ros2rag.starter/src/main/jastadd/Computation.jrag @@ -26,7 +26,7 @@ aspect Computation { return false; } - syn double RobotArm.get_AppropriateSpeed() { + syn double RobotArm.getAppropriateSpeed() { return isInSafetyZone() ? 0.4d : 1.0d; } } diff --git a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag new file mode 100644 index 0000000..5317f52 --- /dev/null +++ b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag @@ -0,0 +1,30 @@ +/** + * Version 2020-05-28 + */ +// --- update definitions --- +read Joint.CurrentPosition using ParseLinkState, LinkStateToIntPosition ; +write RobotArm.AppropriateSpeed using CreateSpeedMessage, SerializeRobotConfig ; + +// --- dependency definitions --- +RobotArm.AppropriateSpeed canDependOn Joint.CurrentPosition as dependency1 ; + +// --- mapping definitions --- +ParseLinkState maps byte[] bytes to panda.Linkstate.PandaLinkState {: + return panda.Linkstate.PandaLinkState.parseFrom(bytes); +:} + +SerializeRobotConfig maps config.Robotconfig.RobotConfig rc to byte[] {: + return rc.toByteArray(); +:} + +LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {: + panda.Linkstate.PandaLinkState.Position p = pls.getPos(); + { int i = 0; } + return IntPosition.of((int) p.getPositionX(), (int) p.getPositionY(), (int) p.getPositionZ()); +:} + +CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {: + return config.Robotconfig.RobotConfig.newBuilder() + .setSpeed(speed) + .build(); +:} diff --git a/ros2rag.example/src/main/jastadd/Navigation.jrag b/ros2rag.starter/src/main/jastadd/Navigation.jrag similarity index 100% rename from ros2rag.example/src/main/jastadd/Navigation.jrag rename to ros2rag.starter/src/main/jastadd/Navigation.jrag diff --git a/ros2rag.example/src/main/jastadd/Example.jadd b/ros2rag.starter/src/main/jastadd/RobotModel.jadd similarity index 100% rename from ros2rag.example/src/main/jastadd/Example.jadd rename to ros2rag.starter/src/main/jastadd/RobotModel.jadd diff --git a/ros2rag.starter/src/main/jastadd/RobotModel.relast b/ros2rag.starter/src/main/jastadd/RobotModel.relast new file mode 100644 index 0000000..ea347d0 --- /dev/null +++ b/ros2rag.starter/src/main/jastadd/RobotModel.relast @@ -0,0 +1,13 @@ +Model ::= RobotArm ZoneModel ; + +ZoneModel ::= <Size:IntPosition> SafetyZone:Zone* ; + +Zone ::= Coordinate* ; + +RobotArm ::= Joint* EndEffector /<AppropriateSpeed:double>/ ; + +Joint ::= <Name:String> <CurrentPosition:IntPosition> ; + +EndEffector : Joint; + +Coordinate ::= <Position:IntPosition> ; diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java new file mode 100644 index 0000000..81fd5bc --- /dev/null +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java @@ -0,0 +1,125 @@ +package de.tudresden.inf.st.ros2rag.starter; + +import de.tudresden.inf.st.ros2rag.starter.ast.*; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; + +import java.io.IOException; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.TimeUnit; + +/** + * Testing Ros2Rag without generating something. + * + * @author rschoene - Initial contribution + */ +public class Main { + + private static final Logger logger = LogManager.getLogger(Main.class); + + public static void main(String[] args) throws IOException, InterruptedException { + // parse arguments + final String mqttHost; + if (args.length > 0) { + mqttHost = args[0]; + } else { + mqttHost = "localhost"; + } + + /* assumed configuration: + panda_ground: ground-plate + panda_link_0: link_0 of the panda + panda_link_1: link_1 of the panda + panda_link_2: link_2 of the panda + panda_link_3: link_3 of the panda + panda_link_4: link_4 of the panda + panda_link_5: link_5 of the panda + panda_link_6: link_6 of the panda + panda_link_7: link_7 of the panda (end effector) + panda_link_8: link_8 of the panda (left finger) + panda_link_9: link_9 of the panda (right finger) + */ + final int numberOfJoints = 10; + final int endEffectorIndex = 7; + final String jointTopicFormat = "panda_link_%d"; + final String configTopic = "robotconfig"; + + final int[][] safetyZoneCoordinates = { + { 0, 0, 0}, + {-1, 0, 0}, + { 1, 0, 0} + }; + + // --- No configuration below this line --- + + Model model = new Model(); + model.MqttSetHost(mqttHost); + + ZoneModel zoneModel = new ZoneModel(); + zoneModel.setSize(makePosition(1, 1, 1)); + + Zone safetyZone = new Zone(); + for (int[] coordinate : safetyZoneCoordinates) { + safetyZone.addCoordinate(new Coordinate( + IntPosition.of(coordinate[0], coordinate[1], coordinate[2]))); + } + model.setZoneModel(zoneModel); + model.MqttWaitUntilReady(2, TimeUnit.SECONDS); + + RobotArm robotArm = new RobotArm(); + model.setRobotArm(robotArm); + + for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { + final Joint jointOrEndEffector; + if (jointIndex == endEffectorIndex) { + EndEffector endEffector = new EndEffector(); + endEffector.setName("gripper"); + robotArm.setEndEffector(endEffector); + jointOrEndEffector = endEffector; + } else { + Joint joint = new Joint(); + joint.setName("joint" + jointIndex); + robotArm.addJoint(joint); + jointOrEndEffector = joint; + } + jointOrEndEffector.setCurrentPosition(makePosition(0, 0, 0)); + robotArm.addDependency1(jointOrEndEffector); + jointOrEndEffector.connectCurrentPosition(String.format(jointTopicFormat, jointIndex)); + } + + robotArm.connectAppropriateSpeed(configTopic, true); + + logStatus("Start", robotArm); + 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'."); + + MqttUpdater mainHandler = new MqttUpdater("mainHandler"); + mainHandler.setHost(mqttHost, 1883); + mainHandler.waitUntilReady(2, TimeUnit.SECONDS); + mainHandler.newConnection("exit", bytes -> exitCondition.countDown()); + mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm)); + + exitCondition.await(); + + mainHandler.close(); + model.MqttCloseConnections(); + } + + private static void logStatus(String prefix, RobotArm robotArm) { + 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()) { + sb.append(joint.getName()).append(": ").append(joint.getCurrentPosition()).append("\n"); + } + sb.append("endEffector ").append(robotArm.getEndEffector().getName()).append(": ") + .append(robotArm.getEndEffector().getCurrentPosition()); + logger.info(sb.toString()); + } + + private static IntPosition makePosition(int x, int y, int z) { + return IntPosition.of(x, y, z); + } +} diff --git a/ros2rag.example/src/main/proto/dataconfig.proto b/ros2rag.starter/src/main/proto/dataconfig.proto similarity index 100% rename from ros2rag.example/src/main/proto/dataconfig.proto rename to ros2rag.starter/src/main/proto/dataconfig.proto diff --git a/ros2rag.example/src/main/proto/linkstate.proto b/ros2rag.starter/src/main/proto/linkstate.proto similarity index 100% rename from ros2rag.example/src/main/proto/linkstate.proto rename to ros2rag.starter/src/main/proto/linkstate.proto diff --git a/ros2rag.example/src/main/proto/robotconfig.proto b/ros2rag.starter/src/main/proto/robotconfig.proto similarity index 100% rename from ros2rag.example/src/main/proto/robotconfig.proto rename to ros2rag.starter/src/main/proto/robotconfig.proto diff --git a/ros2rag.example/src/main/resources/log4j2.xml b/ros2rag.starter/src/main/resources/log4j2.xml similarity index 100% rename from ros2rag.example/src/main/resources/log4j2.xml rename to ros2rag.starter/src/main/resources/log4j2.xml diff --git a/settings.gradle b/settings.gradle index d6c40a5..edf79d1 100644 --- a/settings.gradle +++ b/settings.gradle @@ -3,6 +3,6 @@ rootProject.name = 'ros2rag' include 'relast.preprocessor' include 'ros2rag.base' include 'ros2rag.tests' -include 'ros2rag.example' +include 'ros2rag.starter' include 'ros2rag.senderstub' include 'ros2rag.receiverstub' diff --git a/starter.sh b/starter.sh new file mode 100755 index 0000000..12d625d --- /dev/null +++ b/starter.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash +./gradlew :ros2rag.starter:installDist +./ros2rag.starter/build/install/ros2rag.starter/bin/ros2rag.starter $@ -- GitLab