From 43be0849a233a09cf6cbd489ec6b276467e955c7 Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Wed, 8 Jul 2020 11:28:49 +0200
Subject: [PATCH] Update goal, fixed bug in mappings and MqttHandler.
- Base: inserted DefaultMappings did not account for existing other mappings, but always use type of token
- Base: MqttHandler was not able to have multiple newConnections for the same topic
- Goal: Changed way how last update is handled (now packed into currentStep)
- Goal: Added StartStep
- Goal: Changed wait to be in milliseconds
---
.../src/main/jastadd/backend/Mappings.jrag | 10 ++--
.../src/main/resources/MqttHandler.jadd | 43 +++++++++--------
ros2rag.base/src/main/resources/mqtt.mustache | 3 +-
ros2rag.common/config.yaml | 8 ++--
.../src/main/jastadd/Computation.jrag | 48 +++++--------------
ros2rag.goal/src/main/jastadd/GoalModel.jadd | 45 +++++++++++++++++
.../src/main/jastadd/GoalModel.relast | 3 +-
.../src/main/jastadd/GoalModel.ros2rag | 15 ++++--
ros2rag.goal/src/main/jastadd/Navigation.jrag | 4 +-
ros2rag.goal/src/main/jastadd/Printing.jrag | 10 ++--
.../inf/st/ros2rag/goal/GoalMain.java | 9 ++--
11 files changed, 123 insertions(+), 75 deletions(-)
diff --git a/ros2rag.base/src/main/jastadd/backend/Mappings.jrag b/ros2rag.base/src/main/jastadd/backend/Mappings.jrag
index fd40385..600ce92 100644
--- a/ros2rag.base/src/main/jastadd/backend/Mappings.jrag
+++ b/ros2rag.base/src/main/jastadd/backend/Mappings.jrag
@@ -111,7 +111,7 @@ aspect Mappings {
// or if no mappings are specified.
// then prepend the suitable default mapping
java.util.List<MappingDefinition> result;
- if (getMappingList().size() == 0 || !getMappingList().get(0).getFromType().isByteArray()) {
+ if (getMappingList().isEmpty() || !getMappingList().get(0).getFromType().isByteArray()) {
result = new java.util.ArrayList();
result.add(suitableDefaultMapping());
result.addAll(getMappingList());
@@ -161,7 +161,9 @@ aspect Mappings {
// --- suitableDefaultMapping ---
syn DefaultMappingDefinition UpdateDefinition.suitableDefaultMapping();
eq ReadFromMqttDefinition.suitableDefaultMapping() {
- String typeName = getToken().getJavaTypeUse().getName();
+ String typeName = getMappingList().isEmpty() ?
+ getToken().getJavaTypeUse().getName() :
+ getMappingList().get(0).getFromType().prettyPrint();
switch(typeName) {
case "int":
case "Integer": return ros2rag().defaultBytesToIntMapping();
@@ -180,7 +182,9 @@ aspect Mappings {
}
}
eq WriteToMqttDefinition.suitableDefaultMapping() {
- String typeName = getToken().getJavaTypeUse().getName();
+ String typeName = getMappingList().isEmpty() ?
+ getToken().getJavaTypeUse().getName() :
+ getMappingList().get(getMappingList().size() - 1).getFromType().prettyPrint();
switch(typeName) {
case "int":
case "Integer": return ros2rag().defaultIntToBytesMapping();
diff --git a/ros2rag.base/src/main/resources/MqttHandler.jadd b/ros2rag.base/src/main/resources/MqttHandler.jadd
index 4700527..c3e98dc 100644
--- a/ros2rag.base/src/main/resources/MqttHandler.jadd
+++ b/ros2rag.base/src/main/resources/MqttHandler.jadd
@@ -21,7 +21,7 @@ public class MqttHandler {
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;
+ private final java.util.Map<String, java.util.List<java.util.function.Consumer<byte[]>>> callbacks;
public MqttHandler() {
this("Ros2Rag");
@@ -81,13 +81,15 @@ public class MqttHandler {
@Override
public void onPublish(org.fusesource.hawtbuf.UTF8Buffer topic, org.fusesource.hawtbuf.Buffer body, org.fusesource.mqtt.client.Callback<org.fusesource.mqtt.client.Callback<Void>> ack) {
String topicString = topic.toString();
- java.util.function.Consumer<byte[]> callback = callbacks.get(topicString);
- if (callback == null) {
+ java.util.List<java.util.function.Consumer<byte[]>> callbackList = callbacks.get(topicString);
+ if (callbackList == null || callbackList.isEmpty()) {
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);
+ for (java.util.function.Consumer<byte[]> callback : callbackList) {
+ callback.accept(message);
+ }
}
ack.onSuccess(null); // always acknowledge message
}
@@ -163,27 +165,30 @@ public class MqttHandler {
public void newConnection(String topic, java.util.function.Consumer<byte[]> callback) {
if (!ready) {
- // TODO should maybe be something more kind than throwing an exception here
+ // should maybe be something more kind than throwing an exception here
throw new IllegalStateException("Updater not ready");
}
// register callback
- callbacks.put(topic, callback);
+ if (callbacks.get(topic) == null) {
+ callbacks.put(topic, new java.util.ArrayList<>());
- // subscribe at broker
- org.fusesource.mqtt.client.Topic[] topicArray = { new org.fusesource.mqtt.client.Topic(topic, this.qos) };
- connection.getDispatchQueue().execute(() -> {
- connection.subscribe(topicArray, new org.fusesource.mqtt.client.Callback<byte[]>() {
- @Override
- public void onSuccess(byte[] qoses) {
- logger.debug("Subscribed to {}, qoses: {}", topic, qoses);
- }
+ // subscribe at broker
+ org.fusesource.mqtt.client.Topic[] topicArray = { new org.fusesource.mqtt.client.Topic(topic, this.qos) };
+ connection.getDispatchQueue().execute(() -> {
+ connection.subscribe(topicArray, new org.fusesource.mqtt.client.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);
- }
+ @Override
+ public void onFailure(Throwable cause) {
+ logger.error("Could not subscribe to {}", topic, cause);
+ }
+ });
});
- });
+ }
+ callbacks.get(topic).add(callback);
}
/**
diff --git a/ros2rag.base/src/main/resources/mqtt.mustache b/ros2rag.base/src/main/resources/mqtt.mustache
index fb2b173..d65ca94 100644
--- a/ros2rag.base/src/main/resources/mqtt.mustache
+++ b/ros2rag.base/src/main/resources/mqtt.mustache
@@ -1,5 +1,6 @@
aspect MQTT {
- private MqttHandler {{rootNodeName}}.{{mqttHandlerField}} = new MqttHandler();
+ private String {{rootNodeName}}.MqttName() { return "Ros2Rag"; }
+ private MqttHandler {{rootNodeName}}.{{mqttHandlerField}} = new MqttHandler(MqttName());
public void {{rootNodeName}}.{{mqttSetHostMethod}}(String host) throws java.io.IOException {
{{mqttHandlerField}}.setHost(host);
}
diff --git a/ros2rag.common/config.yaml b/ros2rag.common/config.yaml
index 8f49855..687a32c 100644
--- a/ros2rag.common/config.yaml
+++ b/ros2rag.common/config.yaml
@@ -24,7 +24,9 @@ panda_mqtt_connector:
panda:
EndEffector: "panda::panda_link7"
goal_poses:
- - position: "0 0 0"
- wait: "3"
- position: "1 1 1"
- wait: "2"
+ wait: "5000"
+ - position: "1 0 1"
+ wait: "3000"
+ - position: "0 0 1"
+ wait: "4000"
diff --git a/ros2rag.goal/src/main/jastadd/Computation.jrag b/ros2rag.goal/src/main/jastadd/Computation.jrag
index c64146e..ffa46d6 100644
--- a/ros2rag.goal/src/main/jastadd/Computation.jrag
+++ b/ros2rag.goal/src/main/jastadd/Computation.jrag
@@ -1,51 +1,26 @@
aspect Computation {
-// syn boolean RobotArm.isInSafetyZone() {
-// System.out.println("isInSafetyZone()");
-// for (Link link : getLinkList()) {
-// if (model().getZoneModel().isInSafetyZone(link.getCurrentPosition())) {
-// return true;
-// }
-// }
-// return model().getZoneModel().isInSafetyZone(getEndEffector().getCurrentPosition());
-// }
-//
-// cache ZoneModel.isInSafetyZone(IntPosition pos);
-// syn boolean ZoneModel.isInSafetyZone(IntPosition pos) {
-// System.out.println("isInSafetyZone(" + pos + ")");
-// for (Zone sz : getSafetyZoneList()) {
-// for (Coordinate coordinate : sz.getCoordinateList()) {
-// if (coordinate.getPosition().equals(pos)) {
-// return true;
-// }
-// }
-// }
-// return false;
-// }
-//
-// syn double RobotArm.getAppropriateSpeed() {
-// return isInSafetyZone() ? 0.4d : 1.0d;
-// }
syn Workflow GoalModel.getWorkflow() {
Workflow result = new Workflow();
- Step lastStep = null;
int index = 0;
+ Step startStep = new StartStep();
+ startStep.setId(index++);
+ result.addStep(startStep);
+ result.setCurrentStep(LastUpdatedInteger.of(startStep.getId()));
+ Step lastStep = startStep;
for (WorkPose workPose : getWorkPoseList()) {
MoveToStep moveToStep = new MoveToStep();
moveToStep.setId(index++);
moveToStep.setPosition(workPose.getPosition());
- if (lastStep != null) {
- lastStep.setSuccessor(moveToStep);
- } else {
- // this is the first move-to-step
- result.setCurrentStep(moveToStep.getId());
- }
+ lastStep.setSuccessor(moveToStep);
result.addStep(moveToStep);
+
WorkStep workStep = new WorkStep();
workStep.setId(index++);
workStep.setDuration(workPose.getDuration());
moveToStep.setSuccessor(workStep);
result.addStep(workStep);
- lastStep = moveToStep;
+
+ lastStep = workStep;
}
FinishedStep finish = new FinishedStep();
finish.setId(index);
@@ -61,6 +36,7 @@ aspect Computation {
}
syn int Step.readyForThisStep();
+ eq StartStep.readyForThisStep() = getSuccessor().getId();
eq MoveToStep.readyForThisStep() {
// check if we have reached the destination yet
if (isNear(model().getRobotState().getCurrentPosition(), getPosition())) {
@@ -71,7 +47,8 @@ aspect Computation {
}
eq WorkStep.readyForThisStep() {
// check if we have "worked" long enough
- if (lastStarted() + getDuration() >= model().getRobotState().getLastUpdate()) {
+ System.out.println("lastStarted: " + lastStarted() + ", duration: " + getDuration() + ", lastUpdate: " + model().getRobotState().getLastUpdate());
+ if (model().getRobotState().getLastUpdate() >= lastStarted() + getDuration()) {
// proceed to next step
return getSuccessor().getId();
}
@@ -87,6 +64,7 @@ aspect Computation {
return currentStep().nextPosition();
}
syn DoublePosition Step.nextPosition();
+ eq StartStep.nextPosition() = null; // next position should not be called before step is advanced
eq MoveToStep.nextPosition() = getPosition();
eq WorkStep.nextPosition() = getPredecessor().nextPosition();
eq FinishedStep.nextPosition() = getPredecessor().nextPosition();
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.jadd b/ros2rag.goal/src/main/jastadd/GoalModel.jadd
index f114361..7dd67fb 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.jadd
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.jadd
@@ -44,4 +44,49 @@ aspect GrammarTypes {
return "(" + x + ", " + y + ", " + z + ")";
}
}
+
+ public class LastUpdatedInteger {
+ private final int value;
+ private final long timestampUpdated;
+
+ private LastUpdatedInteger(int value, long timestampUpdated) {
+ this.value = value;
+ this.timestampUpdated = timestampUpdated;
+ }
+
+ public static LastUpdatedInteger of(int value) {
+ return new LastUpdatedInteger(value, System.currentTimeMillis());
+ }
+
+ public int getValue() {
+ return value;
+ }
+
+ public long getTimestampUpdated() {
+ return timestampUpdated;
+ }
+
+ @Override
+ public boolean equals(Object o) {
+ if (this == o) return true;
+ if (o == null || getClass() != o.getClass()) return false;
+ LastUpdatedInteger that = (LastUpdatedInteger) o;
+ return value == that.value &&
+ timestampUpdated == that.timestampUpdated;
+ }
+
+ @Override
+ public int hashCode() {
+ return java.util.Objects.hash(value, timestampUpdated);
+ }
+
+ @Override
+ public String toString() {
+ return "(" + value + " @ " + timestampUpdated + ")";
+ }
+ }
+
+ refine MQTT String GoalModel.MqttName() {
+ return "GoalModel";
+ }
}
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.relast b/ros2rag.goal/src/main/jastadd/GoalModel.relast
index 8c1b2c3..ada05c6 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.relast
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.relast
@@ -4,11 +4,12 @@ RobotState ::= <CurrentPosition:DoublePosition> <LastUpdate:long> ;
WorkPose ::= <Position:DoublePosition> <Duration:long> ; // Position in [m,m,m], Duration in ms
-Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextPosition:DoublePosition>/ <CurrentStep:int> <CurrentStepStart:long> ; // NextPosition in [m,m,m]; CurrentStepStart in ms; ReadyForThisStep and CurrentStep are step ids
+Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextPosition:DoublePosition>/ <CurrentStep:LastUpdatedInteger> ; // NextPosition in [m,m,m]; CurrentStepStart in ms; ReadyForThisStep and CurrentStep are step ids
abstract Step ::= <Id:int> ; // Started in ms
MoveToStep : Step ::= <Position:DoublePosition> ; // Position in [m,m,m]
WorkStep : Step ::= <Duration:long> ; // Duration in ms
+StartStep : Step ;
FinishedStep : Step ;
rel Step.successor <-> Step.predecessor;
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
index 1e99b20..188b6bf 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
@@ -4,8 +4,11 @@
// --- update definitions ---
read RobotState.CurrentPosition using ParseLinkState, LinkStateToDoublePosition ;
read RobotState.LastUpdate using TickWhenLinkState ;
-write Workflow.ReadyForThisStep ; // |_ Those two roles encode are attribute-driven rewrite (via mqtt)
-read Workflow.CurrentStep ; // |
+
+// Those two roles together encode an attribute-driven rewrite (via mqtt)
+write Workflow.ReadyForThisStep ;
+read Workflow.CurrentStep using ParseLastUpdatedInteger ;
+
write Workflow.NextPosition using CreateTrajectoryMessage, SerializeTrajectory ;
// --- dependency definitions ---
@@ -21,7 +24,7 @@ ParseLinkState maps byte[] bytes to panda.Linkstate.PandaLinkState {:
:}
TickWhenLinkState maps byte[] bytes to long {:
-// System.out.println("TickWhenLinkState");
+ System.out.println("TickWhenLinkState");
return System.currentTimeMillis();
:}
@@ -31,7 +34,7 @@ SerializeTrajectory maps plan.TrajectoryOuterClass.Trajectory t to byte[] {:
:}
LinkStateToDoublePosition maps panda.Linkstate.PandaLinkState pls to DoublePosition {:
-// System.out.println("LinkStateToDoublePosition");
+ System.out.println("LinkStateToDoublePosition");
panda.Linkstate.PandaLinkState.Position p = pls.getPos();
return DoublePosition.of(p.getPositionX(), p.getPositionY(), p.getPositionZ());
:}
@@ -46,3 +49,7 @@ CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Traj
.build())
.build();
:}
+
+ParseLastUpdatedInteger maps int value to LastUpdatedInteger {:
+ return LastUpdatedInteger.of(value);
+:}
diff --git a/ros2rag.goal/src/main/jastadd/Navigation.jrag b/ros2rag.goal/src/main/jastadd/Navigation.jrag
index 56d596c..90630ee 100644
--- a/ros2rag.goal/src/main/jastadd/Navigation.jrag
+++ b/ros2rag.goal/src/main/jastadd/Navigation.jrag
@@ -6,7 +6,7 @@ aspect Navigation {
// eq RobotArm.getLink().containingRobotArm() = this;
// eq RobotArm.getEndEffector().containingRobotArm() = this;
syn Step Workflow.currentStep() {
- return resolveStep(getCurrentStep());
+ return resolveStep(getCurrentStep().getValue());
}
syn Step Workflow.resolveStep(int id) {
@@ -23,5 +23,5 @@ aspect Navigation {
eq GoalModel.getChild().model() = this;
inh long Step.lastStarted();
- eq Workflow.getStep().lastStarted() = getCurrentStepStart();
+ eq Workflow.getStep().lastStarted() = getCurrentStep().getTimestampUpdated();
}
diff --git a/ros2rag.goal/src/main/jastadd/Printing.jrag b/ros2rag.goal/src/main/jastadd/Printing.jrag
index cc2c91b..b82655b 100644
--- a/ros2rag.goal/src/main/jastadd/Printing.jrag
+++ b/ros2rag.goal/src/main/jastadd/Printing.jrag
@@ -1,6 +1,8 @@
aspect Printing {
- syn String Step.prettyPrint();
- eq MoveToStep.prettyPrint() = getId() + "Move to " + getPosition();
- eq WorkStep.prettyPrint() = getId() + "Work " + getDuration();
- eq FinishedStep.prettyPrint() = getId() + "Finish";
+ syn String Step.prettyPrint() = getId() + ": " + prettyPrint0() + " ->[" + (getSuccessor() == null ? "/" : getSuccessor().getId()) + "]";
+ syn String Step.prettyPrint0();
+ eq StartStep.prettyPrint0() = "Start";
+ eq MoveToStep.prettyPrint0() = "Move to " + getPosition();
+ eq WorkStep.prettyPrint0() = "Work " + getDuration();
+ eq FinishedStep.prettyPrint0() = "Finish";
}
diff --git a/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java b/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
index 39e94bb..75db626 100644
--- a/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
+++ b/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
@@ -81,9 +81,11 @@ public class GoalMain {
robotState.connectLastUpdate(topic);
}
}, config);
- model.getWorkflow().connectCurrentStep(config.topics.nextStep);
- model.getWorkflow().connectReadyForThisStep(config.topics.nextStep, false);
+ // next position is not initialized, so don't send it
model.getWorkflow().connectNextPosition(config.topics.trajectory, false);
+ model.getWorkflow().connectCurrentStep(config.topics.nextStep);
+ // initial next step is sent, as soon as this is received, the workflow starts
+ model.getWorkflow().connectReadyForThisStep(config.topics.nextStep, true);
logStatus("Start");
CountDownLatch exitCondition = new CountDownLatch(1);
@@ -123,7 +125,8 @@ public class GoalMain {
sb.append(" ").append(step.prettyPrint()).append("\n");
}
sb.append("CurrentStep: ").append(model.getWorkflow().getCurrentStep())
- .append(", lastStart: ").append(model.getWorkflow().getCurrentStepStart()).append("\n");
+// .append(", lastStart: ").append(model.getWorkflow().getCurrentStepStart())
+ .append(", readyForThisStep: ").append(model.getWorkflow().getReadyForThisStep()).append("\n");
sb.append("CurrentPosition: ").append(model.getRobotState().getCurrentPosition())
.append(", lastUpdate: ").append(model.getRobotState().getLastUpdate()).append("\n");
logger.info(sb.toString());
--
GitLab