diff --git a/ros2rag.example/src/main/jastadd/Computation.jrag b/ros2rag.example/src/main/jastadd/Computation.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..11af4952a8f2390a5bcec26c8a3098c7d19f42d2
--- /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 b72ddd50ee92242e425145b373ad32d304f8d87b..09517aa1e47ded59a979b7fbc7d50d7449a3a0d0 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 0c0fce1b28b8d85447d4603101f1526a6a62e778..3b1ca622c18876985a8e945328df529a211d308b 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 0000000000000000000000000000000000000000..918d61f24265e4475c99d93e37afb7359f3095be
--- /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 6641c840b1166876f1d89e6c6a3823ecc3340d72..3ac9639e22337a0163495a8a60f05b195f14f348 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 56d326a696fea1019783120b2437ce371a4367dc..f439cc850cec29f3be705912a89beb7e696383f2 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 8606a19ed07ed77b57b71be4464b490d73441f19..0bfc1776b883dd3aaca90df14a71bc2759f3783b 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 cc9be87ca4aeb63085144b2b9cbd16b8049c7e14..f08ce43f6c71d089f9c932e92c1d3e07148319c9 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 0000000000000000000000000000000000000000..c3e0862e0216e5ea00f79966dff8d5ce434f3dac
--- /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 a3d0ae1ed41d1401b4a4cacab9848e5fa1df4654..5eda5a0db1d914f7a9c5f02ca9594353a8776a59 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;