diff --git a/goal.sh b/goal.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7f5c04923a49bf659e1a9f2680c0507e55de48f3
--- /dev/null
+++ b/goal.sh
@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+./gradlew :ros2rag.goal:installDist
+./ros2rag.goal/build/install/ros2rag.goal/bin/ros2rag.goal $@
diff --git a/ros2rag.common/proto/trajectory.proto b/ros2rag.common/proto/trajectory.proto
new file mode 100644
index 0000000000000000000000000000000000000000..2724b79c3ba3ed20efa99b6c66e6d415e8f3075d
--- /dev/null
+++ b/ros2rag.common/proto/trajectory.proto
@@ -0,0 +1,14 @@
+syntax = "proto3";
+
+package plan;
+
+message Trajectory {
+
+ message Position {
+ double x = 1;
+ double y = 2;
+ double z = 3;
+ }
+
+ repeated Position pos = 1;
+}
diff --git a/ros2rag.goal/.gitignore b/ros2rag.goal/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..87b4cdd3d7c6a41502ca98703abeeb69a1d536fb
--- /dev/null
+++ b/ros2rag.goal/.gitignore
@@ -0,0 +1,5 @@
+build
+src/gen-res/
+src/gen/
+out/
+*.class
diff --git a/ros2rag.goal/build.gradle b/ros2rag.goal/build.gradle
new file mode 100644
index 0000000000000000000000000000000000000000..c6ee36c57fce4032365deb999b3e72393dd31b9c
--- /dev/null
+++ b/ros2rag.goal/build.gradle
@@ -0,0 +1,146 @@
+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'
+ }
+}
diff --git a/ros2rag.goal/src/main/jastadd/Computation.jrag b/ros2rag.goal/src/main/jastadd/Computation.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..c64146e8de760553aaf7aef779c80d94306137b2
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/Computation.jrag
@@ -0,0 +1,101 @@
+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;
+ }
+
+}
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.jadd b/ros2rag.goal/src/main/jastadd/GoalModel.jadd
new file mode 100644
index 0000000000000000000000000000000000000000..f114361784e4a9db8f8ac0674408220999108399
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.jadd
@@ -0,0 +1,47 @@
+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 + ")";
+ }
+ }
+}
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.relast b/ros2rag.goal/src/main/jastadd/GoalModel.relast
new file mode 100644
index 0000000000000000000000000000000000000000..8c1b2c33425c4e0fe27e100063fadf886d24fee1
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.relast
@@ -0,0 +1,15 @@
+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;
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
new file mode 100644
index 0000000000000000000000000000000000000000..1e99b20fc0f176e60e7621931180a53a19ca1515
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
@@ -0,0 +1,48 @@
+/**
+ * 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();
+:}
diff --git a/ros2rag.goal/src/main/jastadd/Navigation.jrag b/ros2rag.goal/src/main/jastadd/Navigation.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..56d596c2a33dcb6d33bb4028fee173b4a61db01a
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/Navigation.jrag
@@ -0,0 +1,27 @@
+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();
+}
diff --git a/ros2rag.goal/src/main/jastadd/Printing.jrag b/ros2rag.goal/src/main/jastadd/Printing.jrag
new file mode 100644
index 0000000000000000000000000000000000000000..cc2c91bbbb17d3bd885d1bb41765b4fad7f362cf
--- /dev/null
+++ b/ros2rag.goal/src/main/jastadd/Printing.jrag
@@ -0,0 +1,6 @@
+aspect Printing {
+ syn String Step.prettyPrint();
+ eq MoveToStep.prettyPrint() = getId() + "Move to " + getPosition();
+ eq WorkStep.prettyPrint() = getId() + "Work " + getDuration();
+ eq FinishedStep.prettyPrint() = getId() + "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
new file mode 100644
index 0000000000000000000000000000000000000000..39e94bbe1043e6d0c2aab981d1d1a1b07ba7d7d8
--- /dev/null
+++ b/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
@@ -0,0 +1,141 @@
+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);
+ }
+}
diff --git a/ros2rag.goal/src/main/proto b/ros2rag.goal/src/main/proto
new file mode 120000
index 0000000000000000000000000000000000000000..bb7d2bae29902b994e92987325a878a0fe318c69
--- /dev/null
+++ b/ros2rag.goal/src/main/proto
@@ -0,0 +1 @@
+../../../ros2rag.common/proto/
\ No newline at end of file
diff --git a/ros2rag.goal/src/main/resources/log4j2.xml b/ros2rag.goal/src/main/resources/log4j2.xml
new file mode 100644
index 0000000000000000000000000000000000000000..98d9c02e1bef12b4efb000298de7903f3ae8bf74
--- /dev/null
+++ b/ros2rag.goal/src/main/resources/log4j2.xml
@@ -0,0 +1,13 @@
+<?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>
diff --git a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/ReceiverMain.java b/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/ReceiverMain.java
index 54b8458adb378b5d63c7dee3617cee1738acf926..8487e621e28b26dd5abc7b78757f797f6f56a411 100644
--- a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/ReceiverMain.java
+++ b/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);
+ Util.setMqttHost(receiver::setHost, config);
receiver.waitUntilReady(2, TimeUnit.SECONDS);
receiver.newConnection(config.topics.robotConfig, this::printRobotConfig);
receiver.newConnection(config.topics.dataConfig, this::printDataConfig);
+ receiver.newConnection(config.topics.trajectory, this::printTrajectory);
+ receiver.newConnection(config.topics.nextStep, this::printNextStep);
Util.iterateLinks((isEndEffector, topic, name) -> {
receiver.newConnection(topic, this::printPandaLinkState);
@@ -124,7 +128,31 @@ public class ReceiverMain {
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
+ }
+
+ private void printTrajectory(byte[] bytes) {
+ try {
+ logger.debug("Got a trajectory message, parsing ...");
+ if (logger.isInfoEnabled()) {
+ Trajectory trajectory = Trajectory.parseFrom(bytes);
+ StringBuilder sb = new StringBuilder("trajectory:");
+ for (Trajectory.Position pos : trajectory.getPosList()) {
+ sb.append(" pos(")
+ .append(String.format("% .5f,", pos.getX()))
+ .append(String.format("% .5f,", pos.getY()))
+ .append(String.format("% .5f", pos.getZ()))
+ .append(")");
+ }
+ logger.info(sb.toString());
+ }
+ } catch (InvalidProtocolBufferException e) {
+ e.printStackTrace();
+ }
+ }
+ private void printNextStep(byte[] bytes) {
+ logger.info("nextStep: {}",
+ java.nio.ByteBuffer.wrap(bytes).getInt());
}
}