Commit 43be0849 authored by René Schöne's avatar René Schöne
Browse files

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
parent b9a3384e
Pipeline #7221 passed with stages
in 5 minutes and 39 seconds
......@@ -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();
......
......@@ -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);
}
/**
......
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);
}
......
......@@ -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"
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();
......
......@@ -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";
}
}
......@@ -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;
......
......@@ -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);
:}
......@@ -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();
}
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";
}
......@@ -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());
......
Markdown is supported
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