Commit 503cf384 authored by René Schöne's avatar René Schöne
Browse files

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
parent 0f8bf9c8
Pipeline #6280 passed with stage
in 1 minute and 47 seconds
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();
}
}
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 ;
......
......@@ -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();
}
......
aspect Navigation {
inh Model RobotArm.model();
eq Model.getRobotArm().model() = this;
}
......@@ -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);
......
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;
}
}
......@@ -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()));
......
......@@ -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);
}
});
}
}
syntax = "proto3";
package config;
message RobotConfig {
double speed = 1;
bool loopTrajectory = 2;
enum PlanningMode {
FLUID = 0;
CARTESIAN = 1;
}
PlanningMode planningMode = 3;
}
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment