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

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();

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,14 +81,16 @@ 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));
+          for (java.util.function.Consumer<byte[]> callback : callbackList) {
             callback.accept(message);
           }
+        }
         ack.onSuccess(null);  // always acknowledge message
       }
 
@@ -163,11 +165,12 @@ 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) };
@@ -185,6 +188,8 @@ public class MqttHandler {
         });
       });
     }
+    callbacks.get(topic).add(callback);
+  }
 
   /**
    * Waits until this updater is ready to receive MQTT messages.

ros2rag.base/src/main/resources/mqtt.mustache

 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);
   }

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"

ros2rag.goal/src/main/jastadd/Computation.jrag

 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());
-      }
       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();

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";
+  }
 }

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;

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);
+:}

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();
 }

ros2rag.goal/src/main/jastadd/Printing.jrag

 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";
 }

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());