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