diff --git a/ros2rag.example/src/main/jastadd/Computation.jrag b/ros2rag.example/src/main/jastadd/Computation.jrag index 11af4952a8f2390a5bcec26c8a3098c7d19f42d2..029d55c884be6ff3bcb70e913a636bebca053518 100644 --- a/ros2rag.example/src/main/jastadd/Computation.jrag +++ b/ros2rag.example/src/main/jastadd/Computation.jrag @@ -8,17 +8,17 @@ aspect Computation { return model().getZoneModel().isInSafetyZone(getEndEffector().getCurrentPosition()); } - cache ZoneModel.isInSafetyZone(Position pos); - syn boolean ZoneModel.isInSafetyZone(Position pos) { + cache ZoneModel.isInSafetyZone(IntPosition pos); + syn boolean ZoneModel.isInSafetyZone(IntPosition 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()) { + for (Coordinate coordinate : sz.getCoordinateList()) { + IntPosition inside = coordinate.getPosition(); + if (inside.getX() <= pos.getX() && + inside.getY() <= pos.getY() && + inside.getZ() <= pos.getZ() && + pos.getX() <= inside.getX() + getSize().getX() && + pos.getY() <= inside.getY() + getSize().getY() && + pos.getZ() <= inside.getZ() + getSize().getZ()) { return true; } } @@ -26,6 +26,11 @@ aspect Computation { return false; } + syn double RobotArm.get_AppropriateSpeed() { + return isInSafetyZone() ? 0.4d : 1.0d; + } + + // this should be a NTA-terminal "getAppropriateSpeed()" syn config.Robotconfig.RobotConfig RobotArm.createSlowSpeedMessage() { return config.Robotconfig.RobotConfig.newBuilder() .setSpeed(isInSafetyZone() ? 0.4d : 1.0d) diff --git a/ros2rag.example/src/main/jastadd/Example.jadd b/ros2rag.example/src/main/jastadd/Example.jadd new file mode 100644 index 0000000000000000000000000000000000000000..1b862a675e0b2963944d7573aca1fd6c13985b06 --- /dev/null +++ b/ros2rag.example/src/main/jastadd/Example.jadd @@ -0,0 +1,42 @@ +aspect GrammarTypes { + public class IntPosition { + private final int x, y, z; + + private IntPosition(int x, int y, int z) { + this.x = x; + this.y = y; + this.z = z; + } + + public static IntPosition of(int x, int y, int z) { + return new IntPosition(x, y, z); + } + + public int getX() { + return x; + } + + public int getY() { + return y; + } + + public int getZ() { + return z; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + IntPosition that = (IntPosition) o; + return x == that.x && + y == that.y && + z == that.z; + } + + @Override + public int hashCode() { + return java.util.Objects.hash(x, y, z); + } + } +} diff --git a/ros2rag.example/src/main/jastadd/Example.relast b/ros2rag.example/src/main/jastadd/Example.relast index 09517aa1e47ded59a979b7fbc7d50d7449a3a0d0..8fc963cf2c200e841b41f9fb4312b6d926381478 100644 --- a/ros2rag.example/src/main/jastadd/Example.relast +++ b/ros2rag.example/src/main/jastadd/Example.relast @@ -1,17 +1,14 @@ Model ::= RobotArm ZoneModel ; -ZoneModel ::= <Size:Position> SafetyZone:Zone* ; +ZoneModel ::= <Size:IntPosition> SafetyZone:Zone* ; -Zone ::= PositionWrapper* ; +Zone ::= Coordinate* ; // Do not use terminal-NTA's for now, as relast has problems with it "/<ShouldUseLowSpeed:Boolean>/" ; -RobotArm ::= Joint* EndEffector <_AttributeTestSource:int> ; // normally this would be: <AttributeTestSource:int> ; +RobotArm ::= Joint* EndEffector <_AttributeTestSource:int> /<_AppropriateSpeed:double>/ ; // normally this would be: <AttributeTestSource:int> ; -Joint ::= <Name> <CurrentPosition:Position> ; -//rel Joint.CurrentPosition -> Position_Old ; +Joint ::= <Name> <_CurrentPosition:IntPosition> ; EndEffector : Joint; -Position_Old ::= <x:int> <y:int> <z:int> ; -PositionWrapper ::= <Position:Position> ; - +Coordinate ::= <Position:IntPosition> ; diff --git a/ros2rag.example/src/main/jastadd/Generated.jrag b/ros2rag.example/src/main/jastadd/Generated.jrag index 3b1ca622c18876985a8e945328df529a211d308b..62677f910c6d7de680d0929e0f9695a87dfa3b09 100644 --- a/ros2rag.example/src/main/jastadd/Generated.jrag +++ b/ros2rag.example/src/main/jastadd/Generated.jrag @@ -3,58 +3,117 @@ 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 GrammarExtension { - // kind of private NTA typed "MqttRoot" and named "_MqttRoot" - syn nta MqttRoot Model.get_MqttRoot() { - return new MqttRoot(); +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.updateMqttHost(String host) throws java.io.IOException { - get_MqttRoot().updateHost(host); + public void Model.MqttSetHost(String host, int port) throws java.io.IOException { + _mqttUpdater.setHost(host, port); } - public void Model.updateMqttHost(String host, int port) throws java.io.IOException { - get_MqttRoot().updateHost(host, port); + public boolean Model.MqttWaitUntilReady(long time, java.util.concurrent.TimeUnit unit) { + return _mqttUpdater.waitUntilReady(time, unit); } - public boolean Model.waitUntilReady(long time, java.util.concurrent.TimeUnit unit) { - return get_MqttRoot().getUpdater().waitUntilReady(time, unit); + public void Model.MqttCloseConnections() { + _mqttUpdater.close(); } inh MqttUpdater Joint._mqttUpdater(); inh MqttUpdater RobotArm._mqttUpdater(); - eq Model.getRobotArm()._mqttUpdater() = get_MqttRoot().getUpdater(); - eq Model.getZoneModel()._mqttUpdater() = get_MqttRoot().getUpdater(); + eq Model.getRobotArm()._mqttUpdater() = _mqttUpdater; + eq Model.getZoneModel()._mqttUpdater() = _mqttUpdater; } -// this aspect is generic and will be always generated in the same way -aspect Mqtt { - // --- default values --- - private static final int MqttRoot.DEFAULT_PORT = 1883; +aspect GrammarExtension { + // --- Joint --- + public void Joint.connectCurrentPosition(String topic) { + _mqttUpdater().newConnection(topic, message -> { + // 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 [...]"? + 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); + try { + containingRobotArm().update_AppropriateSpeed(); + } catch (Exception e) { + // not advisable, but suits for now + e.printStackTrace(); + } + return this; + } - void MqttRoot.updateHost(String host) throws java.io.IOException { - updateHost(host, DEFAULT_PORT); + public IntPosition Joint.getCurrentPosition() { + return get_CurrentPosition(); } - void MqttRoot.updateHost(String host, int port) throws java.io.IOException { - setHost(ExternalHost.of(host, port)); - if (getUpdater() != null) { - // close connection to old updater first - getUpdater().close(); + // --- RobotArm --- + private String RobotArm.write_isInSafetyZone_topic = null; + private String RobotArm.write_AppropriateSpeed_topic = null; + private config.Robotconfig.RobotConfig RobotArm.write_AppropriateSpeed_lastValue = null; + + public RobotArm RobotArm.setAttributeTestSource(int value) { + int old_value = get_AttributeTestSource(); + RobotArm result = 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() ;" + config.Robotconfig.RobotConfig config = createSlowSpeedMessage(); + _mqttUpdater().publish(write_isInSafetyZone_topic, config.toByteArray()); } - setUpdater(new MqttUpdater().setHost(host, port)); + return result; + } + + public int RobotArm.getAttributeTestSource() { + return get_AttributeTestSource(); + } + + public void RobotArm.connect_isInSafetyZone(String topic) { + write_isInSafetyZone_topic = topic; + } + + public void RobotArm.connect_AppropriateSpeed(String topic) { + write_AppropriateSpeed_topic = topic; } - public static ExternalHost ExternalHost.of(String hostName, int defaultPort) { - String host = hostName; - int port = defaultPort; - if (hostName.contains(":")) { - String[] parts = hostName.split(":"); - host = parts[0]; - port = Integer.parseInt(parts[1]); + public void RobotArm.update_AppropriateSpeed() { + // todo here: 1) read _AppropriateSpeed (which triggers computation, right?) + // 2) maybe: check if value has changed since last publish + // 3) publish + 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; } - return new ExternalHost(host, port); + // we know, it is a protobuf type, so call "toByteArray" + byte[] bytes = y.toByteArray(); + _mqttUpdater().publish(write_AppropriateSpeed_topic, bytes); } - syn String ExternalHost.urlAsString() = String.format("http://%s:%s", getHostName(), getPort()); } diff --git a/ros2rag.example/src/main/jastadd/Generated.relast b/ros2rag.example/src/main/jastadd/Generated.relast index 7c6b9db0fe73fc86677f33ec7e0cdc446fb0943c..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 100644 --- a/ros2rag.example/src/main/jastadd/Generated.relast +++ b/ros2rag.example/src/main/jastadd/Generated.relast @@ -1,3 +0,0 @@ -MqttRoot ::= [Host:ExternalHost] <Updater:MqttUpdater> ; -ExternalHost ::= <HostName:String> <Port:int> ; - diff --git a/ros2rag.example/src/main/jastadd/Navigation.jrag b/ros2rag.example/src/main/jastadd/Navigation.jrag index 918d61f24265e4475c99d93e37afb7359f3095be..855a2d87d6bbb1374c5ae23569435fe2a9b329ac 100644 --- a/ros2rag.example/src/main/jastadd/Navigation.jrag +++ b/ros2rag.example/src/main/jastadd/Navigation.jrag @@ -1,4 +1,8 @@ aspect Navigation { inh Model RobotArm.model(); eq Model.getRobotArm().model() = this; + + inh RobotArm Joint.containingRobotArm(); + eq RobotArm.getJoint().containingRobotArm() = this; + eq RobotArm.getEndEffector().containingRobotArm() = 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 3ac9639e22337a0163495a8a60f05b195f14f348..9dd585ecc7d1c3c5479d6d9c41c708afc237c4a5 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 @@ -1,6 +1,7 @@ package de.tudresden.inf.st.ros2rag.example; import com.google.protobuf.InvalidProtocolBufferException; +import de.tudresden.inf.st.ros2rag.ast.IntPosition; import de.tudresden.inf.st.ros2rag.ast.Joint; import panda.Linkstate.PandaLinkState; import panda.Linkstate.PandaLinkState.Position; @@ -16,11 +17,12 @@ public class GeneratedJoint extends Joint { Input for this to be generated: // when an update of pose is read via mqtt, then update current position - [always] read Joint.CurrentPosition using LinkStateToPosition; + [always] read Joint.CurrentPosition using LinkStateToIntPosition; // panda.LinkState is a datatype defined in protobuf - LinkStateToPosition: map panda.Linkstate.PandaLinkState x to Position y using { - y = x.getPos(); + LinkStateToIntPosition: map panda.Linkstate.PandaLinkState x to IntPosition y using { + panda.Linkstate.PandaLinkState.Position p = x.getPos(); + y = IntPosition.of((int) p.getPositionX(), (int) p.getPositionY(), (int) p.getPositionZ()); } */ public void connectCurrentPosition(String topic) { @@ -30,7 +32,11 @@ public class GeneratedJoint extends Joint { // 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(); + 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 setCurrentPosition(y); } catch (InvalidProtocolBufferException e) { e.printStackTrace(); @@ -38,4 +44,30 @@ public class GeneratedJoint extends Joint { }); } + /* + next try with token-attribute "_AppropriateSpeed" (maybe the underscore is not needed) + + (A) [always] write RobotArm._AppropriateSpeed using CreateSpeedMessage ; // _AppropriateSpeed is a token-attribute, and CreateSpeedMessage is a mapping (use) + (B1) RobotArm._AppropriateSpeed dependsOn RobotArm.getJoint().CurrentPosition ; // dependency from token of subtree of robot-arm to token-attribute of robot-arm + (B2) Joint.CurrentPosition changes containingRobotArm()._AppropriateSpeed ; // switched dependency-edge, now from source (left) to target (right), enables attributes for navigation + + CreateSpeedMessage: map double x to protobuf config.Robotconfig.RobotConfig y { + y = config.Robotconfig.RobotConfig.newBuilder() + .setSpeed(x) + .build(); + } + + details to remember: we probably need to distinguish the target. JM claims to only set token-attribute, but could also be normal attributes we want to send away (no difference, both need to be re-computed) + */ + public GeneratedJoint setCurrentPosition(IntPosition value) { + set_CurrentPosition(value); + // (the cast won't be necessary if generated into RobotArm) + ((GeneratedRobotArm) containingRobotArm()).update_AppropriateSpeed(); + return this; + } + + public IntPosition getCurrentPosition() { + return get_CurrentPosition(); + } + } 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 f439cc850cec29f3be705912a89beb7e696383f2..55a50d9d9abeafb5ab83d37e2c1277a0cf758e0e 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 @@ -36,6 +36,8 @@ public class GeneratedRobotArm extends RobotArm { 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; + private String /*GeneratedRobotArm.*/write_AppropriateSpeed_topic = null; + private Robotconfig.RobotConfig /*GeneratedRobotArm.*/write_AppropriateSpeed_lastValue = null; public RobotArm setAttributeTestSource(int value) { int old_value = super.get_AttributeTestSource(); @@ -57,4 +59,27 @@ public class GeneratedRobotArm extends RobotArm { public void connect_isInSafetyZone(String topic) { write_isInSafetyZone_topic = topic; } + + public void connect_AppropriateSpeed(String topic) { + write_AppropriateSpeed_topic = topic; + } + + public void update_AppropriateSpeed() { + // todo here: 1) read _AppropriateSpeed (which triggers computation, right?) + // 2) maybe: check if value has changed since last publish + // 3) publish + double x = get_AppropriateSpeed(); + // begin transformation "CreateSpeedMessage" + Robotconfig.RobotConfig y = Robotconfig.RobotConfig.newBuilder() + .setSpeed(x) + .build(); + // end transformation "CreateSpeedMessage" + if (y.equals(write_AppropriateSpeed_lastValue)) { + // if always is absent, then return here + return; + } + // we know, it is a protobuf type, so call "toByteArray" + byte[] bytes = y.toByteArray(); + _mqttUpdater().publish(write_AppropriateSpeed_topic, bytes); + } } 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 0bfc1776b883dd3aaca90df14a71bc2759f3783b..bd7628efa8fe5559da187735d3595921f6c61c45 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 @@ -1,8 +1,6 @@ package de.tudresden.inf.st.ros2rag.example; -import com.google.protobuf.InvalidProtocolBufferException; import de.tudresden.inf.st.ros2rag.ast.*; -import panda.Linkstate.PandaLinkState.Position; import java.io.IOException; import java.util.concurrent.TimeUnit; @@ -15,44 +13,45 @@ import java.util.concurrent.TimeUnit; public class Main { public static void main(String[] args) throws IOException, InterruptedException { Model model = new Model(); - model.updateMqttHost("localhost"); + model.MqttSetHost("localhost"); ZoneModel zoneModel = new ZoneModel(); zoneModel.setSize(makePosition(1, 1, 1)); - Position myPosition = makePosition(0, 0, 0); - PositionWrapper myPositionWrapper = new PositionWrapper(myPosition); - PositionWrapper leftPosition = new PositionWrapper(makePosition(-1, 0, 0)); - PositionWrapper rightPosition = new PositionWrapper(makePosition(1, 0, 0)); + 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.addPositionWrapper(myPositionWrapper); - safetyZone.addPositionWrapper(leftPosition); - safetyZone.addPositionWrapper(rightPosition); + safetyZone.addCoordinate(myCoordinate); + safetyZone.addCoordinate(leftPosition); + safetyZone.addCoordinate(rightPosition); zoneModel.addSafetyZone(safetyZone); model.setZoneModel(zoneModel); - GeneratedRobotArm robotArm = new GeneratedRobotArm(); + RobotArm robotArm = new RobotArm(); robotArm.set_AttributeTestSource(1); // set initial value, no trigger - GeneratedJoint joint1 = new GeneratedJoint(); + Joint joint1 = new Joint(); joint1.setName("joint1"); joint1.setCurrentPosition(myPosition); EndEffector endEffector = new EndEffector(); endEffector.setName("gripper"); - endEffector.setCurrentPosition(myPosition); + endEffector.setCurrentPosition(myPosition); // fixme: of course, this should be without "_" once generated robotArm.addJoint(joint1); robotArm.setEndEffector(endEffector); model.setRobotArm(robotArm); - model.waitUntilReady(2, TimeUnit.SECONDS); + model.MqttWaitUntilReady(2, TimeUnit.SECONDS); joint1.connectCurrentPosition("robot/joint1"); robotArm.connect_isInSafetyZone("robot/config"); System.out.println("BEFORE joint1.getCurrentPosition() = " + stringify(joint1.getCurrentPosition())); + System.out.println("Now invoke ./send_one.sh"); Thread.sleep(3000); robotArm.setAttributeTestSource(1); @@ -62,19 +61,14 @@ public class Main { System.out.println("AFTER joint1.getCurrentPosition() = " + stringify(joint1.getCurrentPosition())); - // TODO close/shutdown should be exposed - model.get_MqttRoot().getUpdater().close(); + model.MqttCloseConnections(); } - private static Position makePosition(int x, int y, int z) { - return Position.newBuilder() - .setPositionX(x) - .setPositionY(y) - .setPositionZ(z) - .build(); + private static IntPosition makePosition(int x, int y, int z) { + return IntPosition.of(x, y, z); } - private static String stringify(Position position) { - return "(" + position.getPositionX() + ", " + position.getPositionY() + ", " + position.getPositionZ() + ")"; + private static String stringify(IntPosition position) { + return "(" + position.getX() + ", " + position.getY() + ", " + position.getZ() + ")"; } } 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 f08ce43f6c71d089f9c932e92c1d3e07148319c9..bc854112d309175a54f47a98f49376d5fcd235f0 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 @@ -135,6 +135,10 @@ public class MqttUpdater { return this; } + public URI getHost() { + return host; + } + private void throwIf(AtomicReference<Throwable> error) throws IOException { if (error.get() != null) { throw new IOException(error.get()); @@ -192,7 +196,6 @@ public class MqttUpdater { return false; } - public void close() { if (connection == null) { logger.warn("Stopping without connection. Was setHost() called?");