Skip to content
Snippets Groups Projects
Commit ecefc480 authored by Johannes Mey's avatar Johannes Mey
Browse files

initial version of motion grammar working with real robot

parent 07b4d219
No related branches found
No related tags found
No related merge requests found
Pipeline #14089 failed
......@@ -3,6 +3,7 @@ plugins {
id 'application'
id 'idea'
id 'org.jastadd' version "${jastaddgradle_version}"
id "com.google.protobuf" version "${protobuf_plugin_version}"
}
group 'de.tudresden.inf.st'
......@@ -16,6 +17,13 @@ application.mainClass = "${mainClassName}"
java.toolchain.languageVersion = JavaLanguageVersion.of(11)
protobuf {
protoc {
// The artifact spec for the Protobuf Compiler
artifact = "com.google.protobuf:protoc:${protobuf_version}"
}
}
repositories {
mavenCentral()
maven {
......@@ -28,6 +36,7 @@ configurations {
// add a configuration to store the grammar printing dependency in
grammar2uml
relast
ragconnect
}
File genSrc = file("src/gen/java")
......@@ -46,6 +55,11 @@ dependencies {
implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: '2.13.3'
implementation group: 'com.fasterxml.jackson.core', name: 'jackson-core', version: '2.13.3'
implementation group: 'org.slf4j', name: 'slf4j-simple', version: '1.7.31'
ragconnect group: 'de.tudresden.inf.st', name: 'ragconnect', version: '1.0.0-alpha-203'
jastadd2 "org.jastadd:jastadd2:2.3.5-dresden-6"
implementation group: 'com.google.protobuf', name: 'protobuf-java', version: "${protobuf_version}"
implementation group: 'com.google.protobuf', name: 'protobuf-java-util', version: "${protobuf_version}"
implementation group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
}
def versionFile = 'src/main/resources/Version.properties'
......@@ -97,6 +111,33 @@ def loadingGrammarDiagramFile = './src/gen/resources/diagrams/grammar/loading.pn
def cleaningGrammarDiagramFile = './src/gen/resources/diagrams/grammar/cleaning.png'
def jastAddListName = 'JastAddList'
// phases: ragConnect -> RelAst -> JastAdd
// phase: ragConnect
task ragConnect(type: JavaExec) {
group = 'Build'
main = 'org.jastadd.ragconnect.compiler.Compiler'
classpath = configurations.ragconnect
args([
'--o=src/gen/jastadd',
'./src/main/jastadd/common/MotionGrammar.relast',
'./src/main/jastadd/cleanup/RobotWorld.relast',
'./src/main/jastadd/cleanup/RobotWorld.connect',
'--logReads',
'--logWrites',
// '--logIncremental',
'--verbose',
'--rootNode=RobotScene',
'--protocols=mqtt',
"--List=${jastAddListName}",
'--experimental-jastadd-329',
'--incremental=param',
'--tracing=cache,flush',
'--evaluationCounter'
])
}
task generateLoadingGrammarDiagrams(type: JavaExec) {
group = 'Documentation'
classpath = configurations.grammar2uml
......@@ -128,6 +169,7 @@ task relastToJastAdd(type: JavaExec) {
'--useJastAddNames',
'--listClass=java.util.ArrayList',
"--jastAddList=${jastAddListName}",
"--resolverHelper",
'--file'
args relastFiles
......@@ -170,11 +212,15 @@ jastadd {
buildInfoDir = 'src/gen-res'
// default options are: '--rewrite=cnta', '--safeLazy', '--visitCheck=false', '--cacheCycle=false'
extraJastAddOptions = ["--List=${jastAddListName}"]
extraJastAddOptions = ["--List=${jastAddListName}",
"--flush=api",
"--incremental=param,debug",
"--tracing=cache,flush"]
}
// Workflow configuration for phases
clean.dependsOn cleanGen
generateAst.dependsOn relastToJastAdd
generateAst.dependsOn generateLoadingGrammarDiagrams, generateCleaningGrammarDiagrams
relastToJastAdd.dependsOn ragConnect
......@@ -5,3 +5,5 @@ assertj_version = 3.22.0
grammar2uml_version = 0.2.1
jastaddgradle_version = 1.14.5
javalin_version = 4.6.3
protobuf_version = 4.0.0-rc-2
protobuf_plugin_version = 0.8.18
// --- receiving ---
receive RobotScene.Table using ParseScene, ConvertScene ;
ParseScene maps byte[] bytes to de.tudresden.inf.st.ceti.Scene {:
return de.tudresden.inf.st.ceti.Scene.parseFrom(bytes);
:}
ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Table {:
var result = new Table();
result.setRobot(new Robot());
for (var pbObject : pbScene.getObjectsList()) {
PhysicalObject obj;
if (pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX || pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BIN) {
PhysicalObject physicalObject = pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX ? new MovableObject() : new Bin();
physicalObject.setName(pbObject.getId());
java.awt.Color color = new java.awt.Color(pbObject.getColor().getR(), pbObject.getColor().getG(), pbObject.getColor().getB());
physicalObject.setColor("#" + Integer.toHexString(color.getRGB()).substring(2));
Pose pose = new Pose();
pose.setX(pbObject.getPos().getX());
pose.setY(pbObject.getPos().getY());
pose.setZ(pbObject.getPos().getZ());
pose.setQX(pbObject.getOrientation().getX());
pose.setQY(pbObject.getOrientation().getY());
pose.setQZ(pbObject.getOrientation().getZ());
pose.setQW(pbObject.getOrientation().getW());
physicalObject.setPose(pose);
if (pbObject.getOwner() != "" && pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.BOX) {
((MovableObject) physicalObject).setAttachedRobot(result.getRobot());
}
result.addPhysicalObject(physicalObject);
} else if (pbObject.getType() == de.tudresden.inf.st.ceti.Object.Type.ARM) {
result.getRobot().setIsIdle(pbObject.getState() == de.tudresden.inf.st.ceti.Object.State.STATE_IDLE);
}
}
return result;
:}
aspect RobotWorld {
public static RobotScene RobotScene.initialWorld(java.util.Random r) {
public boolean RobotScene.simulated = true;
public org.fusesource.mqtt.client.BlockingConnection RobotScene.connection;
public static RobotScene RobotScene.initialWorld(java.util.Random random) {
Table t = new Table();
t.addPhysicalObject(new Bin("binRed", Pose.of(-10, -10, 0), "red"));
t.addPhysicalObject(new Bin("binGreen", Pose.of(-10, 0, 0), "green"));
t.addPhysicalObject(new MovableObject("boxRed", Pose.of(-5, -10, 0), "red"));
t.addPhysicalObject(new MovableObject("boxGreen", Pose.of(-5, 0, 0), "green"));
Robot b = new Robot();
b.setIsIdle(true);
return new RobotScene(t, b);
Robot r = new Robot();
r.setIsIdle(true);
t.setRobot(r);
return new RobotScene(t);
}
public static RobotScene RobotScene.initialWorld() {
Robot r = new Robot();
r.setIsIdle(true);
Table t = new Table();
t.setRobot(r);
RobotScene result = new RobotScene(t);
result.simulated = false;
return result;
}
public static TokenType EmptyTable.type() { return TokenType.of("EMPTY_TABLE"); }
......@@ -151,7 +165,7 @@ aspect RobotWorld {
}
public RobotIsIdle RobotScene.parseRobotIsIdle() {
System.out.print("Trying to parse token <RobotIsIdle>... ");
if (getRobot().getIsIdle()) {
if (getTable().getRobot().getIsIdle()) {
System.out.println("success");
return new RobotIsIdle();
} else {
......@@ -165,7 +179,7 @@ aspect RobotWorld {
}
public RobotIsNotIdle RobotScene.parseRobotIsNotIdle() {
System.out.print("Trying to parse token <RobotIsNotIdle>... ");
if (!getRobot().getIsIdle()) {
if (!getTable().getRobot().getIsIdle()) {
System.out.println("success");
return new RobotIsNotIdle();
} else {
......@@ -179,7 +193,7 @@ aspect RobotWorld {
}
public RobotHasItemAttached RobotScene.parseRobotHasItemAttached() {
System.out.print("Trying to parse token <RobotHasItemAttached>... ");
if (getRobot().hasAttachedItem()) {
if (getTable().getRobot().hasAttachedItem()) {
System.out.println("success");
return new RobotHasItemAttached();
} else {
......@@ -193,7 +207,7 @@ aspect RobotWorld {
}
public RobotHasNoItemAttached RobotScene.parseRobotHasNoItemAttached() {
System.out.print("Trying to parse token <RobotHasNoItemAttached>... ");
if (!getRobot().hasAttachedItem()) {
if (!getTable().getRobot().hasAttachedItem()) {
System.out.println("success");
return new RobotHasNoItemAttached();
} else {
......
// World Model of the Motion Grammar
RobotScene : World ::= Table Robot;
RobotScene : World ::= Table;
Table ::= PhysicalObject*;
Table ::= PhysicalObject* Robot;
abstract PhysicalObject ::= <Name> Pose <Color>;
MovableObject : PhysicalObject;
Bin : PhysicalObject;
......
......@@ -4,15 +4,29 @@ aspect SemanticActions {
RobotScene scene = world.asRobotScene();
System.out.println("performing semantic action for element Pick");
String objectName = object();
if (scene.simulated) {
java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
scene.getRobot().setIsIdle(false);
try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
scene.getTable().getRobot().setIsIdle(false);
try {
Thread.sleep(1200);
} catch (InterruptedException e) { /* ignore */ }
MovableObject object = scene.getTable().getMovableObjectByName(objectName);
scene.getRobot().setAttachedItem(object);
scene.getTable().getRobot().setAttachedItem(object);
object.setPose(Pose.of(-1, -1, -1));
try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
scene.getRobot().setIsIdle(true);
try {
Thread.sleep(1200);
} catch (InterruptedException e) { /* ignore */ }
scene.getTable().getRobot().setIsIdle(true);
});
} else {
de.tudresden.inf.st.ceti.Command c = de.tudresden.inf.st.ceti.Command.newBuilder().setPick(de.tudresden.inf.st.ceti.Pick.newBuilder().setIdRobot("arm").setIdPick(object()).build()).build();
try {
scene.connection.publish("/ceti_cell_placeworld/command", c.toByteArray(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false);
} catch (Exception e) {
e.printStackTrace();
}
}
}
public void DropObjectAtRightPlace.action(World world) {
......@@ -20,16 +34,29 @@ aspect SemanticActions {
System.out.println("performing semantic action for element Pick");
String objectName = object();
String placeName = place();
if (scene.simulated) {
java.util.concurrent.Executors.newSingleThreadExecutor().submit(() -> {
scene.getRobot().setIsIdle(false);
try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
scene.getTable().getRobot().setIsIdle(false);
try {
Thread.sleep(1200);
} catch (InterruptedException e) { /* ignore */ }
MovableObject object = scene.getTable().getMovableObjectByName(objectName);
Bin bin = scene.getTable().getBinByName(placeName);
object.setPose(Pose.of(bin.getPose()));
scene.getRobot().setAttachedItem(null);
try { Thread.sleep(1200); } catch (InterruptedException e) { /* ignore */ }
scene.getRobot().setIsIdle(true);
scene.getTable().getRobot().setAttachedItem(null);
try {
Thread.sleep(1200);
} catch (InterruptedException e) { /* ignore */ }
scene.getTable().getRobot().setIsIdle(true);
});
} else {
de.tudresden.inf.st.ceti.Command c = de.tudresden.inf.st.ceti.Command.newBuilder().setDrop(de.tudresden.inf.st.ceti.Drop.newBuilder().setIdRobot("arm").setIdBin(place()).build()).build();
try {
scene.connection.publish("/ceti_cell_placeworld/command", c.toByteArray(), org.fusesource.mqtt.client.QoS.AT_LEAST_ONCE, false);
} catch (Exception e) {
e.printStackTrace();
}
}
}
public void Wait.action(World world) {
......
......@@ -24,21 +24,30 @@ aspect Tracing {
World.printContextOf("Pose.setZ()-DONE", this, de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
}
refine
public RobotScene RobotScene.setTable(Table t) {
refined(t);
World.printContextOf("RobotScene.setTable()-DONE", getTable(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
return this;
}
// this is probably never called because each child overrides the setter method
refine
public void PhysicalObject.setPose(Pose p) {
public PhysicalObject PhysicalObject.setPose(Pose p) {
World.printContextOf("PhysicalObject.setPose()-BEFORE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.REMOVE_COLOR);
refined(p);
World.printContextOf("PhysicalObject.setPose()-AFTER", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.ADD_COLOR);
World.printContextOf("PhysicalObject.setPose()-DONE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
return this;
}
refine
public void MovableObject.setPose(Pose p) {
public MovableObject MovableObject.setPose(Pose p) {
World.printContextOf("MovableObject.setPose()-BEFORE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.REMOVE_COLOR);
refined(p);
World.printContextOf("MovableObject.setPose()-AFTER", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.ADD_COLOR);
World.printContextOf("MovableObject.setPose()-DONE", getPose(), de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR);
return this;
}
refine
......
......@@ -65,8 +65,7 @@ public abstract class MotionGrammarParser<T extends MotionGrammarElement> {
.customPreamble("title " + step)
.skinParam(SkinParamBooleanSetting.Shadowing, false)
.skinParam(SkinParamStringSetting.backgroundColor, "white")
.dumpAsSVG(svgPath)
.dumpAsPNG(MotionGrammarConfig.astDiagramDir.resolve("AST-" + String.format("%03d", timeStep_) + "-" + new SimpleDateFormat("yyyy.MM.dd.HH.mm.ss.SSS").format(new java.util.Date()) + "-" + step + ".png"));
.dumpAsSVG(svgPath);
DiagramProvider.getInstance().publishAst(new Date(), timeStep_, step, svgPath);
timeStep_++;
} catch (IOException e) {
......
// connector.proto
// this file contains the messages that are exchanged between the ROS connector and other systems
syntax = "proto3";
option java_package = "de.tudresden.inf.st.ceti";
option java_multiple_files = true;
message Object {
// Position is object-center related
message Position {
double x = 1; // in m
double y = 2; // in m
double z = 3; // height in m
}
// 3D description of the object
message Size {
double length = 1; // in m
double width = 2; // in m
double height = 3; // in m
}
message Orientation {
double x = 1; // normalized quaternion
double y = 2;
double z = 3;
double w = 4;
}
message Color {
float r = 1; // 0..1
float g = 2; // 0..1
float b = 3; // 0..1
}
enum Type {
UNKNOWN = 0;
BOX = 1;
BIN = 2;
ARM = 3;
DROP_OFF_LOCATION = 4;
HUMAN = 5;
ROBOT = 6;
COLLABORATION_ZONE = 7;
}
enum State {
STATE_UNKNOWN = 0;
// robot states
STATE_IDLE = 101;
STATE_PICKING = 102;
STATE_PLACING = 103;
STATE_MOVING = 104;
// object
STATE_STATIONARY = 200;
STATE_PICKED = 201;
}
// determines the meaning of a scene object when the scene has is_delta flag
enum Mode {
MODE_UNKNOWN = 0; // if and only if is_delta = false, this should be the value of the field
MODE_KEEP = 1; // the object's properties do not change, thus don't need to be processed by the receiver
MODE_MODIFY = 2; // the object's properties have changed
MODE_ADD = 3; // the object did not exist in the scene before
MODE_REMOVE = 4; // the object is removed from the scene, its properties can be ignored
}
string id = 1;
Type type = 2;
Position pos = 3;
Size size = 4;
Orientation orientation = 5;
Color color = 6;
State state = 7;
string owner = 8;
Mode mode = 9;
}
// the scene is stored within the ROS side and sent to clients
message Scene {
repeated Object objects = 1;
bool is_delta = 2;
}
// the selection is done by a client and sent to ROS
message Selection {
string id = 1; // the id corresponds to an id of an Object in a Scene
}
message Command {
oneof msg {
PickAndPlace pickAndPlace = 1;
ConfigChange configChange = 2;
Evacuate evacuate = 3;
Pick pick = 4;
Place place = 5;
Drop drop = 6;
}
}
message PickAndPlace {
string idRobot = 1; // id of the robot that should execute this operation
string idPick = 2; // id of the object in the scene to be picked
string idPlace = 3; // id of the location the picked object shall be placed.
}
message ConfigChange {
string idCollaborationZone = 1; // id of collaboration zone to change
string idRobotNewOwner = 2; // id of robot that will become new owner
}
message Evacuate {
string idRobot = 1; // id of robot that need to move out of its currently defined collision objects
string idCollaborationZone = 2; // id of collaboration zone to evacuate
}
message Pick {
string idRobot = 1; // id of the robot that should execute this operation
string idPick = 2; // id of the object in the scene to be picked
}
message Place {
string idRobot = 1; // id of the robot that should execute this operation
string idPlace = 2; // id of the location currently grasped object shall be placed.
}
message Drop {
string idRobot = 1; // id of the robot that should execute this operation
string idBin = 2; // id of the bin the picked object shall be dropped in.
}
\ No newline at end of file
package de.tudresden.inf.st.mg;
import de.tudresden.inf.st.mg.common.MotionGrammarConfig;
import de.tudresden.inf.st.mg.common.MotionGrammarParser;
import de.tudresden.inf.st.mg.jastadd.model.*;
import org.junit.jupiter.api.BeforeAll;
......@@ -8,10 +9,12 @@ import org.junit.jupiter.api.Test;
import java.io.File;
import java.io.IOException;
import java.net.URISyntaxException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Comparator;
import java.util.Random;
import java.util.concurrent.TimeUnit;
import static org.assertj.core.api.Assertions.assertThat;
......@@ -55,7 +58,7 @@ public class ParserTest {
void runTidyParser() throws MotionGrammarParser.ParseException {
// for some reason, the best random seed value here is 1 and not 0???
World world = RobotScene.initialWorld(new Random(1));
RobotScene world = RobotScene.initialWorld(new Random(1));
// create a parser using the world
RobotParser parser = new RobotParser(world);
......@@ -72,4 +75,55 @@ public class ParserTest {
}
}
@Test
@Disabled
void runRealTidyParser() throws MotionGrammarParser.ParseException {
MotionGrammarConfig.astDiagramDir = TIDY_AST_DIAGRAM_DIR;
RobotScene world = RobotScene.initialWorld();
org.fusesource.mqtt.client.MQTT handler = new org.fusesource.mqtt.client.MQTT();
try {
handler.setHost("tcp://localhost:1883");
} catch (URISyntaxException e) {
e.printStackTrace();
}
world.connection = handler.blockingConnection();
try {
world.connection.connect();
} catch (Exception e) {
e.printStackTrace();
}
System.err.println(world.connection.isConnected());
try {
world.connectTable("mqtt://localhost//ceti_cell_empty/scene/update");
} catch (IOException e) {
e.printStackTrace();
}
while (world.getTable().getNumPhysicalObject() == 0) {
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
// create a parser using the world
RobotParser parser = new RobotParser(world);
// parse (synchonously, long-running)
var result = parser.parse();
assertThat(result).isNotNull().isInstanceOf(Tidy.class);
try {
Thread.sleep(100000*1000);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment