Commit 901d7fde authored by René Schöne's avatar René Schöne
Browse files

Added ros2rag.goal as an application.

parent c29bc52d
Pipeline #7218 passed with stages
in 4 minutes and 18 seconds
#!/usr/bin/env bash
./gradlew :ros2rag.goal:installDist
./ros2rag.goal/build/install/ros2rag.goal/bin/ros2rag.goal $@
syntax = "proto3";
package plan;
message Trajectory {
message Position {
double x = 1;
double y = 2;
double z = 3;
}
repeated Position pos = 1;
}
build
src/gen-res/
src/gen/
out/
*.class
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'
}
}
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;
}
}
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 + ")";
}
}
}
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;
/**
* 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();
:}
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();
}
aspect Printing {
syn String Step.prettyPrint();
eq MoveToStep.prettyPrint() = getId() + "Move to " + getPosition();
eq WorkStep.prettyPrint() = getId() + "Work " + getDuration();
eq FinishedStep.prettyPrint() = getId() + "Finish";
}
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.common/proto/
\ No newline at end of file
<?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>
......@@ -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);