Added ros2rag.goal as an application.

goal.sh

+#!/usr/bin/env bash
+./gradlew :ros2rag.goal:installDist
+./ros2rag.goal/build/install/ros2rag.goal/bin/ros2rag.goal $@

ros2rag.common/proto/trajectory.proto

+syntax = "proto3";
+
+package plan;
+
+message Trajectory {
+
+  message Position {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+  }
+
+  repeated Position pos = 1;
+}

ros2rag.goal/.gitignore

+build
+src/gen-res/
+src/gen/
+out/
+*.class

ros2rag.goal/build.gradle

+apply plugin: 'jastadd'
+apply plugin: 'application'
+apply plugin: 'com.google.protobuf'
+
+sourceCompatibility = 1.8
+
+mainClassName = 'de.tudresden.inf.st.ros2rag.goal.GoalMain'
+
+repositories {
+    jcenter()
+}
+
+buildscript {
+    repositories.jcenter()
+    dependencies {
+        classpath 'org.jastadd:jastaddgradle:1.13.3'
+        classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.12'
+    }
+}
+
+configurations {
+    baseRuntimeClasspath
+}
+
+sourceSets.main.java.srcDir "src/gen/java"
+jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.goal.GoalMain')
+
+dependencies {
+    implementation project (':ros2rag.base')
+    implementation project (':ros2rag.common')
+    baseRuntimeClasspath project (':ros2rag.base')
+    api group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: "${jackson_version}"
+    api group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_version}"
+    implementation group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11'
+    api group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0'
+    api group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
+
+    jastadd2 "org.jastadd:jastadd:2.3.4"
+}
+
+test {
+    useJUnitPlatform()
+
+    maxHeapSize = '1G'
+}
+
+// Input files for relast
+def relastFiles = ["src/gen/jastadd/Grammar.relast"]
+
+// phases: Ros2Rag -> RelAst -> JastAdd
+// phase: Ros2Rag
+task ros2rag(type: JavaExec) {
+    classpath = configurations.baseRuntimeClasspath
+
+    group = 'Build'
+    main = 'org.jastadd.ros2rag.compiler.Compiler'
+
+    args([
+            '--verbose',
+            '--outputDir=src/gen/jastadd',
+            '--inputGrammar=src/main/jastadd/GoalModel.relast',
+            '--inputRos2Rag=src/main/jastadd/GoalModel.ros2rag',
+            '--rootNode=GoalModel'
+    ])
+}
+
+// phase: RelAst
+task relastToJastAdd(type: JavaExec) {
+    group = 'Build'
+    main = "-jar"
+
+    args(["../libs/relast.jar",
+            "--grammarName=./src/gen/jastadd/model",
+            "--useJastAddNames",
+            "--listClass=java.util.ArrayList",
+            "--jastAddList=JastAddList",
+            "--resolverHelper",
+            "--file"]
+    +
+            relastFiles)
+
+    inputs.files relastFiles
+    outputs.files file("./src/gen/jastadd/model.ast"), file("./src/gen/jastadd/model.jadd")
+}
+
+// phase: JastAdd
+jastadd {
+    configureModuleBuild()
+    modules {
+        //noinspection GroovyAssignabilityCheck
+        module("ros2rag goal") {
+
+            java {
+                basedir "src/"
+                include "main/**/*.java"
+                include "gen/**/*.java"
+            }
+
+            jastadd {
+                basedir "src/"
+                include "main/jastadd/**/*.ast"
+                include "main/jastadd/**/*.jadd"
+                include "main/jastadd/**/*.jrag"
+                include "gen/jastadd/**/*.ast"
+                include "gen/jastadd/**/*.jadd"
+                include "gen/jastadd/**/*.jrag"
+            }
+        }
+    }
+
+    cleanGen.doFirst {
+        delete "src/gen/java/org"
+        delete "src/gen-res/BuildInfo.properties"
+    }
+
+    preprocessParser.doFirst {
+
+        args += ["--no-beaver-symbol"]
+
+    }
+
+    module = "ros2rag goal"
+
+    astPackage = 'de.tudresden.inf.st.ros2rag.goal.ast'
+
+    genDir = 'src/gen/java'
+
+    buildInfoDir = 'src/gen-res'
+
+    // jastaddOptions = ["--lineColumnNumbers", "--visitCheck=true", "--rewrite=cnta", "--cache=all"]
+    // default options are: '--rewrite=cnta', '--safeLazy', '--visitCheck=false', '--cacheCycle=false'
+    extraJastAddOptions = ["--lineColumnNumbers", '--List=JastAddList']
+}
+
+// Workflow configuration for phases
+generateAst.dependsOn relastToJastAdd
+relastToJastAdd.dependsOn ros2rag
+
+protobuf {
+    // create strange directories, so use default here
+//    generatedFilesBaseDir = "$projectDir/src/gen/java"
+    protoc {
+        // The artifact spec for the Protobuf Compiler
+        artifact = 'com.google.protobuf:protoc:3.0.0'
+    }
+}

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;
+    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;
+    }
+    FinishedStep finish = new FinishedStep();
+    finish.setId(index);
+    result.addStep(finish);
+    if (lastStep != null) {
+      lastStep.setSuccessor(finish);
+    }
+    return result;
+  }
+
+  syn int Workflow.getReadyForThisStep() {
+    return currentStep().readyForThisStep();
+  }
+
+  syn int Step.readyForThisStep();
+  eq MoveToStep.readyForThisStep() {
+    // check if we have reached the destination yet
+    if (isNear(model().getRobotState().getCurrentPosition(), getPosition())) {
+      return getSuccessor().getId();
+    }
+    // we haven't reached the destination, so we are not ready and return our id
+    return getId();
+  }
+  eq WorkStep.readyForThisStep() {
+    // check if we have "worked" long enough
+    if (lastStarted() + getDuration() >= model().getRobotState().getLastUpdate()) {
+      // proceed to next step
+      return getSuccessor().getId();
+    }
+    // we haven't worked long enough, so we are not ready and return our id
+    return getId();
+  }
+  eq FinishedStep.readyForThisStep() {
+    // we always want to stay in this step
+    return getId();
+  }
+
+  syn DoublePosition Workflow.getNextPosition() {
+    return currentStep().nextPosition();
+  }
+  syn DoublePosition Step.nextPosition();
+  eq MoveToStep.nextPosition() = getPosition();
+  eq WorkStep.nextPosition() = getPredecessor().nextPosition();
+  eq FinishedStep.nextPosition() = getPredecessor().nextPosition();
+
+  private static final double MoveToStep.DELTA = 0.1;
+  private boolean MoveToStep.isNear(DoublePosition one, DoublePosition other) {
+    return Math.abs(one.getX() - other.getX()) < DELTA &&
+      Math.abs(one.getY() - other.getY()) < DELTA &&
+      Math.abs(one.getZ() - other.getZ()) < DELTA;
+  }
+
+}

ros2rag.goal/src/main/jastadd/GoalModel.jadd

+aspect GrammarTypes {
+  public class DoublePosition {
+    private final double x, y, z;
+
+    private DoublePosition(double x, double y, double z) {
+      this.x = x;
+      this.y = y;
+      this.z = z;
+    }
+
+    public static DoublePosition of(double x, double y, double z) {
+      return new DoublePosition(x, y, z);
+    }
+
+    public double getX() {
+      return x;
+    }
+
+    public double getY() {
+      return y;
+    }
+
+    public double getZ() {
+      return z;
+    }
+
+    @Override
+    public boolean equals(Object o) {
+      if (this == o) return true;
+      if (o == null || getClass() != o.getClass()) return false;
+      DoublePosition that = (DoublePosition) o;
+      return x == that.x &&
+          y == that.y &&
+          z == that.z;
+    }
+
+    @Override
+    public int hashCode() {
+      return java.util.Objects.hash(x, y, z);
+    }
+
+    @Override
+    public String toString() {
+      return "(" + x + ", " + y + ", " + z + ")";
+    }
+  }
+}

ros2rag.goal/src/main/jastadd/GoalModel.relast

+GoalModel ::= WorkPose* /Workflow/ RobotState ;
+
+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
+
+abstract Step ::= <Id:int> ;  // Started in ms
+MoveToStep : Step ::= <Position:DoublePosition> ;  // Position in [m,m,m]
+WorkStep : Step ::= <Duration:long> ;  // Duration in ms
+FinishedStep : Step ;
+
+rel Step.successor <-> Step.predecessor;
+//rel GoalModel.currentStep? -> Step;

ros2rag.goal/src/main/jastadd/GoalModel.ros2rag

+/**
+ * Version 2020-05-28
+ */
+// --- 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 ;        // |
+write Workflow.NextPosition using CreateTrajectoryMessage, SerializeTrajectory ;
+
+// --- dependency definitions ---
+Workflow.ReadyForThisStep canDependOn RobotState.LastUpdate as LastUpdateDependency ;
+Workflow.ReadyForThisStep canDependOn RobotState.CurrentPosition as PositionDependency ;
+
+Workflow.NextPosition canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
+
+// --- mapping definitions ---
+ParseLinkState maps byte[] bytes to panda.Linkstate.PandaLinkState {:
+//  System.out.println("ParseLinkState");
+  return panda.Linkstate.PandaLinkState.parseFrom(bytes);
+:}
+
+TickWhenLinkState maps byte[] bytes to long {:
+//  System.out.println("TickWhenLinkState");
+  return System.currentTimeMillis();
+:}
+
+SerializeTrajectory maps plan.TrajectoryOuterClass.Trajectory t to byte[] {:
+//  System.out.println("SerializeTrajectory");
+  return t.toByteArray();
+:}
+
+LinkStateToDoublePosition maps panda.Linkstate.PandaLinkState pls to DoublePosition {:
+//  System.out.println("LinkStateToDoublePosition");
+  panda.Linkstate.PandaLinkState.Position p = pls.getPos();
+  return DoublePosition.of(p.getPositionX(), p.getPositionY(), p.getPositionZ());
+:}
+
+CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Trajectory {:
+  // compute intermediate points at ROS side, not here
+  return plan.TrajectoryOuterClass.Trajectory.newBuilder()
+    .addPos(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+      .setX(dp.getX())
+      .setY(dp.getY())
+      .setZ(dp.getZ())
+      .build())
+    .build();
+:}

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

+aspect Navigation {
+//  inh Model RobotArm.model();
+//  eq Model.getRobotArm().model() = this;
+//
+//  inh RobotArm Link.containingRobotArm();
+//  eq RobotArm.getLink().containingRobotArm() = this;
+//  eq RobotArm.getEndEffector().containingRobotArm() = this;
+  syn Step Workflow.currentStep() {
+    return resolveStep(getCurrentStep());
+  }
+
+  syn Step Workflow.resolveStep(int id) {
+    for (Step step : getSteps()) {
+      if (step.getId() == id) {
+        return step;
+      }
+    }
+    System.err.println("Could not resolve step with id " + id);
+    return null;
+  }
+
+  inh GoalModel Step.model();
+  eq GoalModel.getChild().model() = this;
+
+  inh long Step.lastStarted();
+  eq Workflow.getStep().lastStarted() = getCurrentStepStart();
+}

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

ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java

+package de.tudresden.inf.st.ros2rag.goal;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
+import de.tudresden.inf.st.ros2rag.common.DataConfiguration.DataWorkPose;
+import de.tudresden.inf.st.ros2rag.common.Util;
+import de.tudresden.inf.st.ros2rag.goal.ast.*;
+import de.tudresden.inf.st.ros2rag.common.DataConfiguration;
+import de.tudresden.inf.st.ros2rag.common.DataConfiguration.ActualConfiguration;
+import org.apache.logging.log4j.LogManager;
+import org.apache.logging.log4j.Logger;
+import plan.TrajectoryOuterClass;
+
+import java.io.File;
+import java.io.IOException;
+import java.util.concurrent.CountDownLatch;
+import java.util.concurrent.TimeUnit;
+
+/**
+ * Testing Ros2Rag without generating something.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class GoalMain {
+
+  private static final Logger logger = LogManager.getLogger(GoalMain.class);
+  private MqttHandler mainHandler;
+  private GoalModel model;
+
+  public void run(String[] args) throws IOException, InterruptedException {
+    File configFile = new File(args.length == 0 ? "../ros2rag.common/config.yaml" : args[0]);
+
+    // --- No configuration below this line ---
+
+    ObjectMapper mapper = new ObjectMapper(new YAMLFactory());
+    System.out.println("Using config file: " + configFile.getAbsolutePath());
+    ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector;
+
+    model = new GoalModel();
+    Util.setMqttHost(model::MqttSetHost, config);
+
+    for (DataWorkPose dataWorkPose : config.goal_poses) {
+      WorkPose workPose = new WorkPose();
+      double[] position = {0, 0, 0};
+      String[] positionSplit = dataWorkPose.position.split(" ");
+      for (int i = 0; i < positionSplit.length; i++) {
+        position[i] = Double.parseDouble(positionSplit[i]);
+      }
+      workPose.setPosition(DoublePosition.of(position[0], position[1], position[2]));
+      workPose.setDuration(Long.parseLong(dataWorkPose.wait));
+      model.addWorkPose(workPose);
+    }
+    RobotState robotState = new RobotState();
+    robotState.setCurrentPosition(DoublePosition.of(0, 0, 0));
+    robotState.setLastUpdate(0);
+    model.setRobotState(robotState);
+
+    model.MqttWaitUntilReady(2, TimeUnit.SECONDS);
+
+    logger.debug("Setting dependencies");
+    /*
+    Workflow.ReadyForThisStep canDependOn RobotState.LastUpdate as LastUpdateDependency ;
+    Workflow.ReadyForThisStep canDependOn RobotState.CurrentPosition as PositionDependency ;
+    Workflow.NextPosition canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
+     */
+    model.getWorkflow().addLastUpdateDependency(robotState);
+    model.getWorkflow().addPositionDependency(robotState);
+    model.getWorkflow().addOnStepChangeDependency(model.getWorkflow());
+
+    logger.debug("Connecting to topics");
+    /*
+    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 ;        // |
+    write Workflow.NextPosition using CreateTrajectoryMessage, SerializeTrajectory ;
+     */
+    Util.iterateLinks((isEndEffector, topic, name) -> {
+      if (isEndEffector) {
+        robotState.connectCurrentPosition(topic);
+        robotState.connectLastUpdate(topic);
+      }
+    }, config);
+    model.getWorkflow().connectCurrentStep(config.topics.nextStep);
+    model.getWorkflow().connectReadyForThisStep(config.topics.nextStep, false);
+    model.getWorkflow().connectNextPosition(config.topics.trajectory, false);
+
+    logStatus("Start");
+    CountDownLatch exitCondition = new CountDownLatch(1);
+
+    logger.info("To print the current model states, send a message to the topic 'model'.");
+    logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C.");
+
+    mainHandler = new MqttHandler("mainHandler").dontSendWelcomeMessage();
+    Util.setMqttHost(mainHandler::setHost, config);
+    mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
+    mainHandler.newConnection("exit", bytes -> exitCondition.countDown());
+    mainHandler.newConnection("model", bytes -> logStatus(new String(bytes)));
+
+//    sendInitialDataConfig(mainHandler, config.topics.dataConfig);
+
+    Runtime.getRuntime().addShutdownHook(new Thread(this::close));
+
+    exitCondition.await();
+
+    this.close();
+  }
+
+//  private void sendInitialDataConfig(MqttHandler mainHandler, String dataConfigTopic) {
+//    Dataconfig.DataConfig dataConfig = Dataconfig.DataConfig.newBuilder()
+//        .setEnablePosition(true)
+//        .setEnableOrientation(false)
+//        .setEnableTwistAngular(false)
+//        .setEnableTwistLinear(false)
+//        .setPublishRate(100)
+//        .build();
+//    mainHandler.publish(dataConfigTopic, dataConfig.toByteArray(), true);
+//  }
+
+  private void logStatus(String prefix) {
+    StringBuilder sb = new StringBuilder(prefix).append("\n");
+    for (Step step : model.getWorkflow().getSteps()) {
+      sb.append(" ").append(step.prettyPrint()).append("\n");
+    }
+    sb.append("CurrentStep: ").append(model.getWorkflow().getCurrentStep())
+        .append(", lastStart: ").append(model.getWorkflow().getCurrentStepStart()).append("\n");
+    sb.append("CurrentPosition: ").append(model.getRobotState().getCurrentPosition())
+        .append(", lastUpdate: ").append(model.getRobotState().getLastUpdate()).append("\n");
+    logger.info(sb.toString());
+  }
+
+  private void close() {
+    logger.info("Exiting ...");
+    mainHandler.close();
+    model.MqttCloseConnections();
+  }
+
+  public static void main(String[] args) throws IOException, InterruptedException {
+    new GoalMain().run(args);
+  }
+}

ros2rag.goal/src/main/proto

+../../../ros2rag.common/proto/
\ No newline at end of file

ros2rag.goal/src/main/resources/log4j2.xml

+<?xml version="1.0" encoding="UTF-8"?>
+<Configuration status="INFO">
+    <Appenders>
+        <Console name="Console" target="SYSTEM_OUT">
+            <PatternLayout pattern="%d{HH:mm:ss.SSS} [%t] %-5level %logger{1.} - %msg%n"/>
+        </Console>
+    </Appenders>
+    <Loggers>
+        <Root level="debug">
+            <AppenderRef ref="Console"/>
+        </Root>
+    </Loggers>
+</Configuration>

ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/ReceiverMain.java

@@ -5,14 +5,16 @@ import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
 import com.google.protobuf.InvalidProtocolBufferException;
 import config.Dataconfig.DataConfig;
 import config.Robotconfig.RobotConfig;
-import de.tudresden.inf.st.ros2rag.starter.Util;
 import de.tudresden.inf.st.ros2rag.starter.ast.MqttHandler;
-import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration;
-import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration.ActualConfiguration;
+import de.tudresden.inf.st.ros2rag.common.Util;
+import de.tudresden.inf.st.ros2rag.common.DataConfiguration;
+import de.tudresden.inf.st.ros2rag.common.DataConfiguration.ActualConfiguration;
 import org.apache.logging.log4j.Level;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 import panda.Linkstate.PandaLinkState;
+import plan.TrajectoryOuterClass;
+import plan.TrajectoryOuterClass.Trajectory;
 
 import java.io.File;
 import java.io.IOException;
@@ -39,10 +41,12 @@ public class ReceiverMain {
 
     AtomicInteger topicMaxLength = new AtomicInteger();
     MqttHandler receiver = new MqttHandler("receiver stub");
-    Util.setMqttHost(receiver, config);