From 503cf38452a4851eb48c73f755fe40a850264f4c Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Wed, 15 Apr 2020 17:22:06 +0200
Subject: [PATCH] Small example of a virtual token.
- Example: add integer token to test. using the position would mix both operators (which should be possible, but is the difficult case)
- Example: add could-be-generated methods for the robot arm
- Example: testing to set the token in Main
- Example: added publish method to generic MqttUpdater
- Example: copied protobuf definition for robotconfig
---
.../src/main/jastadd/Computation.jrag | 35 +++++++++++++
.../src/main/jastadd/Example.relast | 6 +--
.../src/main/jastadd/Generated.jrag | 1 +
.../src/main/jastadd/Navigation.jrag | 4 ++
.../st/ros2rag/example/GeneratedJoint.java | 6 ++-
.../st/ros2rag/example/GeneratedRobotArm.java | 49 +++++++++++++++++++
.../inf/st/ros2rag/example/Main.java | 14 ++++--
.../inf/st/ros2rag/example/MqttUpdater.java | 17 ++++++-
.../src/main/proto/robotconfig.proto | 16 ++++++
.../inf/st/ros2rag/senderstub/Main.java | 2 -
10 files changed, 138 insertions(+), 12 deletions(-)
create mode 100644 ros2rag.example/src/main/jastadd/Computation.jrag
create mode 100644 ros2rag.example/src/main/jastadd/Navigation.jrag
create mode 100644 ros2rag.example/src/main/proto/robotconfig.proto
diff --git a/ros2rag.example/src/main/jastadd/Computation.jrag b/ros2rag.example/src/main/jastadd/Computation.jrag
new file mode 100644
index 0000000..11af495
--- /dev/null
+++ b/ros2rag.example/src/main/jastadd/Computation.jrag
@@ -0,0 +1,35 @@
+aspect Computation {
+ syn boolean RobotArm.isInSafetyZone() {
+ for (Joint joint : getJointList()) {
+ if (model().getZoneModel().isInSafetyZone(joint.getCurrentPosition())) {
+ return true;
+ }
+ }
+ return model().getZoneModel().isInSafetyZone(getEndEffector().getCurrentPosition());
+ }
+
+ cache ZoneModel.isInSafetyZone(Position pos);
+ syn boolean ZoneModel.isInSafetyZone(Position pos) {
+ for (Zone sz : getSafetyZoneList()) {
+ for (PositionWrapper szp : sz.getPositionWrapperList()) {
+ Position inside = szp.getPosition();
+ if (inside.getPositionX() <= pos.getPositionX() &&
+ inside.getPositionY() <= pos.getPositionY() &&
+ inside.getPositionZ() <= pos.getPositionZ() &&
+ pos.getPositionX() <= inside.getPositionX() + getSize().getPositionX() &&
+ pos.getPositionY() <= inside.getPositionY() + getSize().getPositionY() &&
+ pos.getPositionZ() <= inside.getPositionZ() + getSize().getPositionZ()) {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ syn config.Robotconfig.RobotConfig RobotArm.createSlowSpeedMessage() {
+ return config.Robotconfig.RobotConfig.newBuilder()
+ .setSpeed(isInSafetyZone() ? 0.4d : 1.0d)
+ .build();
+ }
+
+}
diff --git a/ros2rag.example/src/main/jastadd/Example.relast b/ros2rag.example/src/main/jastadd/Example.relast
index b72ddd5..09517aa 100644
--- a/ros2rag.example/src/main/jastadd/Example.relast
+++ b/ros2rag.example/src/main/jastadd/Example.relast
@@ -1,11 +1,11 @@
Model ::= RobotArm ZoneModel ;
-ZoneModel ::= <Size:Position> SafetyZone:Zone*;
+ZoneModel ::= <Size:Position> SafetyZone:Zone* ;
-Zone ::= PositionWrapper*;
+Zone ::= PositionWrapper* ;
// Do not use terminal-NTA's for now, as relast has problems with it "/<ShouldUseLowSpeed:Boolean>/" ;
-RobotArm ::= Joint* EndEffector ;
+RobotArm ::= Joint* EndEffector <_AttributeTestSource:int> ; // normally this would be: <AttributeTestSource:int> ;
Joint ::= <Name> <CurrentPosition:Position> ;
//rel Joint.CurrentPosition -> Position_Old ;
diff --git a/ros2rag.example/src/main/jastadd/Generated.jrag b/ros2rag.example/src/main/jastadd/Generated.jrag
index 0c0fce1..3b1ca62 100644
--- a/ros2rag.example/src/main/jastadd/Generated.jrag
+++ b/ros2rag.example/src/main/jastadd/Generated.jrag
@@ -22,6 +22,7 @@ aspect GrammarExtension {
}
inh MqttUpdater Joint._mqttUpdater();
+ inh MqttUpdater RobotArm._mqttUpdater();
eq Model.getRobotArm()._mqttUpdater() = get_MqttRoot().getUpdater();
eq Model.getZoneModel()._mqttUpdater() = get_MqttRoot().getUpdater();
}
diff --git a/ros2rag.example/src/main/jastadd/Navigation.jrag b/ros2rag.example/src/main/jastadd/Navigation.jrag
new file mode 100644
index 0000000..918d61f
--- /dev/null
+++ b/ros2rag.example/src/main/jastadd/Navigation.jrag
@@ -0,0 +1,4 @@
+aspect Navigation {
+ inh Model RobotArm.model();
+ eq Model.getRobotArm().model() = this;
+}
diff --git a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedJoint.java b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedJoint.java
index 6641c84..3ac9639 100644
--- a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedJoint.java
+++ b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedJoint.java
@@ -19,14 +19,16 @@ public class GeneratedJoint extends Joint {
[always] read Joint.CurrentPosition using LinkStateToPosition;
// panda.LinkState is a datatype defined in protobuf
- LinkStateToPosition: map panda.Linkstate x to Position y using {
+ LinkStateToPosition: map panda.Linkstate.PandaLinkState x to Position y using {
y = x.getPos();
}
*/
public void connectCurrentPosition(String topic) {
_mqttUpdater().newConnection(topic, message -> {
- // Parse message into a LinkState
+ // Parse message into a PandaLinkState
try {
+ // TODO the line with parseFrom should be generated, but needs a hint, that the type is protobuf
+ // maybe: "map protobuf panda.Linkstate.PandaLinkState [...]"?
PandaLinkState x = PandaLinkState.parseFrom(message);
Position y = x.getPos();
setCurrentPosition(y);
diff --git a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedRobotArm.java b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedRobotArm.java
index 56d326a..f439cc8 100644
--- a/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedRobotArm.java
+++ b/ros2rag.example/src/main/java/de/tudresden/inf/st/ros2rag/example/GeneratedRobotArm.java
@@ -1,5 +1,6 @@
package de.tudresden.inf.st.ros2rag.example;
+import config.Robotconfig;
import de.tudresden.inf.st.ros2rag.ast.RobotArm;
/**
@@ -8,4 +9,52 @@ import de.tudresden.inf.st.ros2rag.ast.RobotArm;
* @author rschoene - Initial contribution
*/
public class GeneratedRobotArm extends RobotArm {
+ /*
+ // as soon as the cache of isInSafetyZone is invalidated, update the value of Robot.ShouldUseLowSpeed with its value
+ [always] update Robot.ShouldUseLowSpeed with isInSafetyZone() using transformation();
+
+ // when a (new?) value for ShouldUseLowSpeed is set, send it over via mqtt
+ [always] write Robot.ShouldUseLowSpeed;
+
+ ---
+
+ actually, it should look like:
+ (A) [always] write RobotArm.createSpeedMessage() when isInSafetyZone() ; // write attribute value, when the second attribute changes. maybe we should not allow the "when" clause, if we allow attribute dependencies
+ (B) RobotArm.isInSafetyZone() dependsOn RobotArm.AttributeTestSource ; // explicit dependency for implicit instance of the same RobotArm
+ (C) RobotArm.createSpeedMessage() dependsOn RobotArm.isInSafetyZone() ; // explicit attribute dependency
+
+ interesting detail: an attribute has no object, and can not be connected, but a generated method like
+ "connect_isInSafetyZone(String topic)" can :)
+
+ - the line "TYPE.ATTRIBUTE() dependsOn TYPE.TOKEN" implies: 1) change TOKEN to _TOKEN, 2) generate "virtual" setter/getter for TOKEN
+ - the implications of line "TYPE.ATTRIBUTE_1() dependsOn TYPE.ATTRIBUTE_2()" are currently unclear
+ - the line "write TYPE.ATTRIBUTE_1() when ATTRIBUTE_2()" implies: 1) add a private field for the topic to publish, 2) add a "connect_ATTRIBUTE_1(String topic)" method to set the topic, 3) inject a trigger in every setter the ATTRIBUTE_2 depends on, to check for a change (unless "always" was specified), store the result of ATTRIBUTE_1 in a variable, and publish this variable for the stored topic
+ */
+
+ /*
+ 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 /*GeneratedRobotArm.*/write_isInSafetyZone_topic = null;
+
+ public RobotArm setAttributeTestSource(int value) {
+ int old_value = super.get_AttributeTestSource();
+ RobotArm result = super.set_AttributeTestSource(value);
+ // this is a primitive type, so compare with "!=" instead of !equals()
+ if (old_value != value && write_isInSafetyZone_topic != null) {
+ // TODO the method-call "toByteArray" can be generated, if known, that RobotConfig is a protobuf type
+ // maybe "write [...] using protobuf createSlowSpeedMessage() ;"
+ Robotconfig.RobotConfig config = createSlowSpeedMessage();
+ _mqttUpdater().publish(write_isInSafetyZone_topic, config.toByteArray());
+ }
+ return result;
+ }
+
+ public int getAttributeTestSource() {
+ return super.get_AttributeTestSource();
+ }
+
+ public void connect_isInSafetyZone(String topic) {
+ write_isInSafetyZone_topic = topic;
+ }
}
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
index 8606a19..0bfc177 100644
--- 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
@@ -13,7 +13,7 @@ import java.util.concurrent.TimeUnit;
* @author rschoene - Initial contribution
*/
public class Main {
- public static void main(String[] args) throws InvalidProtocolBufferException, IOException, InterruptedException {
+ public static void main(String[] args) throws IOException, InterruptedException {
Model model = new Model();
model.updateMqttHost("localhost");
@@ -30,8 +30,10 @@ public class Main {
safetyZone.addPositionWrapper(leftPosition);
safetyZone.addPositionWrapper(rightPosition);
zoneModel.addSafetyZone(safetyZone);
+ model.setZoneModel(zoneModel);
- RobotArm robotArm = new GeneratedRobotArm();
+ GeneratedRobotArm robotArm = new GeneratedRobotArm();
+ robotArm.set_AttributeTestSource(1); // set initial value, no trigger
GeneratedJoint joint1 = new GeneratedJoint();
joint1.setName("joint1");
@@ -48,9 +50,15 @@ public class Main {
model.waitUntilReady(2, TimeUnit.SECONDS);
joint1.connectCurrentPosition("robot/joint1");
+ robotArm.connect_isInSafetyZone("robot/config");
+
System.out.println("BEFORE joint1.getCurrentPosition() = " + stringify(joint1.getCurrentPosition()));
- Thread.sleep(10000);
+ Thread.sleep(3000);
+ robotArm.setAttributeTestSource(1);
+ Thread.sleep(3000);
+ robotArm.setAttributeTestSource(5);
+ Thread.sleep(3000);
System.out.println("AFTER joint1.getCurrentPosition() = " + stringify(joint1.getCurrentPosition()));
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
index cc9be87..f08ce43 100644
--- 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
@@ -32,8 +32,8 @@ public class MqttUpdater {
/** The connection to the MQTT broker. */
private CallbackConnection connection;
/** Whether we are subscribed to the topics yet */
- private Condition readyCondition;
- private Lock readyLock;
+ private final Condition readyCondition;
+ private final Lock readyLock;
private boolean ready;
private QoS qos;
/** Dispatch knowledge */
@@ -211,4 +211,17 @@ public class MqttUpdater {
});
}
+ 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");
+ }
+
+ @Override
+ public void onFailure(Throwable value) {
+ logger.warn("Could not publish on topic '{}'", topic);
+ }
+ });
+ }
}
diff --git a/ros2rag.example/src/main/proto/robotconfig.proto b/ros2rag.example/src/main/proto/robotconfig.proto
new file mode 100644
index 0000000..c3e0862
--- /dev/null
+++ b/ros2rag.example/src/main/proto/robotconfig.proto
@@ -0,0 +1,16 @@
+syntax = "proto3";
+
+package config;
+
+message RobotConfig {
+
+ double speed = 1;
+ bool loopTrajectory = 2;
+
+ enum PlanningMode {
+ FLUID = 0;
+ CARTESIAN = 1;
+ }
+
+ PlanningMode planningMode = 3;
+}
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 a3d0ae1..5eda5a0 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
@@ -2,8 +2,6 @@ package de.tudresden.inf.st.ros2rag.senderstub;
import panda.Linkstate;
-import java.util.Arrays;
-
public class Main {
public static void main(String[] args) throws Exception {
String topic;
--
GitLab