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

Update to new config and protobuf messages.

- Handling of config and protobuf classes moved to ros2rag.common
parent 23209151
Pipeline #7329 passed with stages
in 5 minutes and 44 seconds
......@@ -43,4 +43,9 @@ subprojects {
testImplementation group: 'org.hamcrest', name: 'hamcrest-junit', version: '2.0.0.0'
}
test {
useJUnitPlatform()
maxHeapSize = '1G'
}
}
apply plugin: 'com.google.protobuf'
sourceCompatibility = 1.8
repositories {
jcenter()
}
buildscript {
repositories.jcenter()
dependencies {
classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.12'
}
}
sourceSets.main.java.srcDir "src/gen/java"
dependencies {
implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: "${jackson_version}"
implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-yaml', version: "${jackson_version}"
api group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0'
}
test {
useJUnitPlatform()
maxHeapSize = '1G'
}
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'
}
}
panda_mqtt_connector:
server: "tcp://localhost:1883"
server: "tcp://0.0.0.0:1883"
robot_speed_factor: .7
robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
default_trajectory: "<none>" # "square" or "circle", everything else is ignored
default_trajectory: "<none>" # "square" or "circle", everything else is ignored
data_publish_rate: 50.0 # publish every x'th robot state message
topics:
robotConfig: "robotconfig"
dataConfig: "dataconfig"
trajectory: "trajectory"
nextStep: "ros2rag/nextStep"
zone_size: 0.5
......@@ -21,15 +21,16 @@ panda_mqtt_connector:
Link4: "panda::panda_link4"
Link5: "panda::panda_link5"
Link6: "panda::panda_link6"
LeftFinger: "panda::panda_leftfinger"
Link7: "panda::panda_link7"
# LeftFinger: "panda::panda_leftfinger"
RightFinger: "panda::panda_rightfinger"
end_effectors:
panda:
EndEffector: "panda::panda_link7"
EndEffector: "panda::panda_leftfinger"
goal_poses:
- position: "0.4 0.3 0.1"
- position: "0.6 0 0.6"
wait: "5000"
- position: "0.4 0.0 0.1"
- position: "-0.6 0.0 0.6"
wait: "3000"
- position: "0.4 -0.3 0.1"
- position: "0.6 0.0 0.6"
wait: "4000"
syntax = "proto3";
package config;
message DataConfig {
bool enablePosition = 1;
bool enableOrientation = 2;
bool enableTwistLinear = 3;
bool enableTwistAngular = 4;
int32 publishRate = 5;
}
syntax = "proto3";
package panda;
message PandaLinkState {
string name = 1;
message Position {
float positionX = 1;
float positionY = 2;
float positionZ = 3;
}
message Orientation {
float orientationX = 1;
float orientationY = 2;
float orientationZ = 3;
float orientationW = 4;
}
message TwistLinear {
float twistLinearX = 1;
float twistLinearY = 2;
float twistLinearZ = 3;
}
message TwistAngular {
float twistAngularX = 1;
float twistAngularY = 2;
float twistAngularZ = 3;
}
Position pos = 2;
Orientation orient = 3;
TwistLinear tl = 4;
TwistAngular ta = 5;
}
package de.tudresden.inf.st.ros2rag.common;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import java.util.List;
import java.util.Map;
import java.util.SortedMap;
......@@ -9,8 +11,10 @@ import java.util.SortedMap;
*
* @author rschoene - Initial contribution
*/
@JsonIgnoreProperties(ignoreUnknown = true)
public class DataConfiguration {
public ActualConfiguration panda_mqtt_connector;
@JsonIgnoreProperties(ignoreUnknown = true)
public static class ActualConfiguration {
public String server = "tcp://localhost:1883";
public DataTopics topics;
......@@ -25,7 +29,6 @@ public class DataConfiguration {
}
public static class DataTopics {
public String robotConfig;
public String dataConfig;
public String trajectory;
public String nextStep;
}
......
package de.tudresden.inf.st.ros2rag.common;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.databind.DeserializationFeature;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
import com.fasterxml.jackson.dataformat.yaml.YAMLParser;
import de.tudresden.inf.st.ros2rag.common.DataConfiguration.ActualConfiguration;
import java.io.File;
import java.io.IOException;
import java.util.Map;
import java.util.SortedMap;
......@@ -13,6 +19,14 @@ import java.util.SortedMap;
*/
public class Util {
public static ActualConfiguration parseConfig(File configFile) throws IOException {
System.out.println("Using config file: " + configFile.getAbsolutePath());
ObjectMapper mapper = new ObjectMapper(
new YAMLFactory().configure(JsonParser.Feature.ALLOW_YAML_COMMENTS, true)
);
return mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector;
}
public static void setMqttHost(SetHost handler, ActualConfiguration config) throws IOException {
HostAndPort hostAndPort = split(config.server);
handler.setHost(hostAndPort.host, hostAndPort.port);
......
......@@ -3,14 +3,5 @@ syntax = "proto3";
package config;
message RobotConfig {
double speed = 1;
bool loopTrajectory = 2;
enum PlanningMode {
FLUID = 0;
CARTESIAN = 1;
}
PlanningMode planningMode = 3;
}
}
\ No newline at end of file
syntax = "proto3";
package robot;
message RobotState {
message Position {
double x = 1;
double y = 2;
double z = 3;
}
message Orientation {
double x = 1;
double y = 2;
double z = 3;
double w = 4;
}
message LinearTwist {
double x = 1;
double y = 2;
double z = 3;
}
message AngularTwist {
double x = 1;
double y = 2;
double z = 3;
}
string name = 1;
Position position = 2;
Orientation orientation = 3;
LinearTwist linear_twist = 4;
AngularTwist angular_twist = 5;
}
\ No newline at end of file
......@@ -10,5 +10,24 @@ message Trajectory {
double z = 3;
}
repeated Position pos = 1;
message Orientation {
double x = 1;
double y = 2;
double z = 3;
double w = 4;
}
enum PlanningMode {
FLUID = 0;
CARTESIAN = 1;
}
message Pose {
Position position = 1;
Orientation orientation = 2;
PlanningMode mode = 3;
}
repeated Pose pose = 1;
bool loop = 2;
}
apply plugin: 'jastadd'
apply plugin: 'application'
apply plugin: 'com.google.protobuf'
sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.goal.GoalMain'
......@@ -14,7 +11,6 @@ buildscript {
repositories.jcenter()
dependencies {
classpath 'org.jastadd:jastaddgradle:1.13.3'
classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.12'
}
}
......@@ -23,27 +19,16 @@ configurations {
}
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"]
......@@ -135,12 +120,3 @@ jastadd {
// 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'
}
}
......@@ -64,7 +64,8 @@ aspect Computation {
return currentStep().nextPosition();
}
syn DoublePosition Step.nextPosition();
eq StartStep.nextPosition() = null; // next position should not be called before step is advanced
// next position should not be called before step is advanced
eq StartStep.nextPosition() = DoublePosition.of(Double.NaN, Double.NaN, Double.NaN);
eq MoveToStep.nextPosition() = getPosition();
eq WorkStep.nextPosition() = getPredecessor().nextPosition();
eq FinishedStep.nextPosition() = getPredecessor().nextPosition();
......
......@@ -2,7 +2,7 @@
* Version 2020-05-28
*/
// --- update definitions ---
read RobotState.CurrentPosition using ParseLinkState, LinkStateToDoublePosition ;
read RobotState.CurrentPosition using ParseRobotState, RobotStateToDoublePosition ;
read RobotState.LastUpdate using TickWhenLinkState ;
// Those two roles together encode an attribute-driven rewrite (via mqtt)
......@@ -18,13 +18,13 @@ Workflow.ReadyForThisStep canDependOn RobotState.CurrentPosition as PositionDepe
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);
ParseRobotState maps byte[] bytes to robot.RobotStateOuterClass.RobotState {:
// System.out.println("ParseRobotState");
return robot.RobotStateOuterClass.RobotState.parseFrom(bytes);
:}
TickWhenLinkState maps byte[] bytes to long {:
System.out.println("TickWhenLinkState");
// System.out.println("TickWhenLinkState");
return System.currentTimeMillis();
:}
......@@ -33,21 +33,23 @@ SerializeTrajectory maps plan.TrajectoryOuterClass.Trajectory t to byte[] {:
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());
RobotStateToDoublePosition maps robot.RobotStateOuterClass.RobotState rs to DoublePosition {:
// System.out.println("RobotStateToDoublePosition");
robot.RobotStateOuterClass.RobotState.Position p = rs.getPosition();
return DoublePosition.of(p.getX(), p.getY(), p.getZ());
:}
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();
.addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
.setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
.setX(dp.getX())
.setY(dp.getY())
.setZ(dp.getZ())
.build())
.build())
.build();
:}
ParseLastUpdatedInteger maps int value to LastUpdatedInteger {:
......
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.ActualConfiguration;
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;
......@@ -32,9 +28,7 @@ public class GoalMain {
// --- 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;
ActualConfiguration config = Util.parseConfig(configFile);
model = new GoalModel();
Util.setMqttHost(model::MqttSetHost, config);
......
../../../ros2rag.common/proto/
\ No newline at end of file
apply plugin: 'application'
apply plugin: 'com.google.protobuf'
sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.receiverstub.ReceiverMain'
......@@ -11,30 +8,11 @@ repositories {
buildscript {
repositories.jcenter()
dependencies {
classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.12'
}
}
sourceSets.main.java.srcDir "src/gen/java"
jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.receiverstub.ReceiverMain')
dependencies {
implementation project(':ros2rag.starter')
implementation project(':ros2rag.common')
}
test {
useJUnitPlatform()
maxHeapSize = '1G'
}
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'
}
}
package de.tudresden.inf.st.ros2rag.receiverstub;
import com.fasterxml.jackson.databind.ObjectMapper;
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.ast.MqttHandler;
import de.tudresden.inf.st.ros2rag.common.Util;
import de.tudresden.inf.st.ros2rag.common.DataConfiguration;
import config.Config.RobotConfig;
import de.tudresden.inf.st.ros2rag.common.DataConfiguration.ActualConfiguration;
import de.tudresden.inf.st.ros2rag.common.Util;
import de.tudresden.inf.st.ros2rag.starter.ast.MqttHandler;
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 robot.RobotStateOuterClass.RobotState;
import java.io.File;
import java.io.IOException;
......@@ -29,10 +24,8 @@ public class ReceiverMain {
public static void main(String[] args) throws Exception {
ReceiverMain main = new ReceiverMain();
ObjectMapper mapper = new ObjectMapper(new YAMLFactory());
File configFile = new File(args[0]);
System.out.println("Using config file: " + configFile.getAbsolutePath());
ActualConfiguration config = mapper.readValue(configFile, DataConfiguration.class).panda_mqtt_connector;
ActualConfiguration config = Util.parseConfig(configFile);
main.run(config);
}
......@@ -44,7 +37,6 @@ public class ReceiverMain {
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);
......@@ -74,29 +66,21 @@ public class ReceiverMain {
private void printPandaLinkState(byte[] bytes) {
try {
logger.debug("Got a joint message, parsing ...");
PandaLinkState pls = PandaLinkState.parseFrom(bytes);
PandaLinkState.Position tmpPosition = pls.getPos();
PandaLinkState.Orientation tmpOrientation = pls.getOrient();
PandaLinkState.TwistLinear tmpTwistLinear = pls.getTl();
PandaLinkState.TwistAngular tmpTwistAngular = pls.getTa();
RobotState pls = RobotState.parseFrom(bytes);
RobotState.Position position = pls.getPosition();
RobotState.Orientation orientation = pls.getOrientation();
RobotState.LinearTwist linearTwist = pls.getLinearTwist();
RobotState.AngularTwist angularTwist = pls.getAngularTwist();
// panda::panda_link0: pos(-3.0621333E-8,-1.5197388E-8,3.3411725E-5), orient(0.0,0.0,0.0,0.0), twist-linear(0.0,0.0,0.0), twist-angular(0.0,0.0,0.0)
logger.printf(Level.INFO,topicPattern + ": pos(% .5f,% .5f,% .5f), ori(%.1f,%.1f,%.1f,%.1f)," +
logger.printf(Level.INFO,topicPattern +
": pos(% .5f,% .5f,% .5f), ori(%.1f,%.1f,%.1f,%.1f)," +
" twL(%.1f,%.1f,%.1f), twA(%.1f,%.1f,%.1f)",
pls.getName(),
tmpPosition.getPositionX(),
tmpPosition.getPositionY(),
tmpPosition.getPositionZ(),
tmpOrientation.getOrientationX(),
tmpOrientation.getOrientationY(),
tmpOrientation.getOrientationZ(),
tmpOrientation.getOrientationW(),
tmpTwistLinear.getTwistLinearX(),
tmpTwistLinear.getTwistLinearY(),
tmpTwistLinear.getTwistLinearZ(),
tmpTwistAngular.getTwistAngularX(),
tmpTwistAngular.getTwistAngularY(),
tmpTwistAngular.getTwistAngularZ());
position.getX(), position.getY(), position.getZ(),
orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW(),
linearTwist.getX(), linearTwist.getY(), linearTwist.getZ(),
angularTwist.getX(), angularTwist.getY(), angularTwist.getZ());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
......@@ -106,25 +90,7 @@ public class ReceiverMain {
try {
logger.debug("Got a robotConfig message, parsing ...");
RobotConfig robotConfig = RobotConfig.parseFrom(bytes);
logger.info("robotConfig: speed = {}, loopTrajectory = {}, planningMode = {}",
robotConfig.getSpeed(),
robotConfig.getLoopTrajectory(),
robotConfig.getPlanningMode().toString());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
}
private void printDataConfig(byte[] bytes) {
try {
logger.debug("Got a dataConfig message, parsing ...");
DataConfig dataConfig = DataConfig.parseFrom(bytes);
logger.info("dataConfig: position = {}, orientation = {}, twistLinear = {}, twistAngular = {}, publishRate = {}",
dataConfig.getEnablePosition(),
dataConfig.getEnableOrientation(),
dataConfig.getEnableTwistLinear(),
dataConfig.getEnableTwistAngular(),
dataConfig.getPublishRate());
logger.info("robotConfig: speed = {}", robotConfig.getSpeed());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
......@@ -135,14 +101,29 @@ public class ReceiverMain {
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(")");
StringBuilder sb = new StringBuilder("trajectory: [");
boolean first = true;
for (Trajectory.Pose pose : trajectory.getPoseList()) {
Trajectory.Position position = pose.getPosition();