diff --git a/build.gradle b/build.gradle
index 5f86f7d094af70ac9fc2241e78dbd8df18c53c4d..1e6f7df9fb271213f83812bc233ae3193af58a44 100644
--- a/build.gradle
+++ b/build.gradle
@@ -43,4 +43,9 @@ subprojects {
testImplementation group: 'org.hamcrest', name: 'hamcrest-junit', version: '2.0.0.0'
}
+ test {
+ useJUnitPlatform()
+ maxHeapSize = '1G'
+ }
+
}
diff --git a/ros2rag.common/build.gradle b/ros2rag.common/build.gradle
new file mode 100644
index 0000000000000000000000000000000000000000..6d0657ba2384835d8ffd8cd8f283535753f96822
--- /dev/null
+++ b/ros2rag.common/build.gradle
@@ -0,0 +1,37 @@
+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'
+ }
+}
diff --git a/ros2rag.common/config.yaml b/ros2rag.common/config.yaml
index e5c9b018dbe4bab7ffc0124edfb184cb81c48e02..889bf22911b2406af758a9083c57a2ebb8d52a45 100644
--- a/ros2rag.common/config.yaml
+++ b/ros2rag.common/config.yaml
@@ -1,11 +1,11 @@
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"
diff --git a/ros2rag.common/proto/dataconfig.proto b/ros2rag.common/proto/dataconfig.proto
deleted file mode 100644
index 472b8c6bda8637ec7f637909b33322933a61a053..0000000000000000000000000000000000000000
--- a/ros2rag.common/proto/dataconfig.proto
+++ /dev/null
@@ -1,13 +0,0 @@
-syntax = "proto3";
-
-package config;
-
-message DataConfig {
-
- bool enablePosition = 1;
- bool enableOrientation = 2;
- bool enableTwistLinear = 3;
- bool enableTwistAngular = 4;
-
- int32 publishRate = 5;
-}
diff --git a/ros2rag.common/proto/linkstate.proto b/ros2rag.common/proto/linkstate.proto
deleted file mode 100644
index dc95138ba49f35497f4bdf061496145168292c9d..0000000000000000000000000000000000000000
--- a/ros2rag.common/proto/linkstate.proto
+++ /dev/null
@@ -1,38 +0,0 @@
-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;
-}
diff --git a/ros2rag.common/proto/robotconfig.proto b/ros2rag.common/proto/robotconfig.proto
deleted file mode 100644
index c3e0862e0216e5ea00f79966dff8d5ce434f3dac..0000000000000000000000000000000000000000
--- a/ros2rag.common/proto/robotconfig.proto
+++ /dev/null
@@ -1,16 +0,0 @@
-syntax = "proto3";
-
-package config;
-
-message RobotConfig {
-
- double speed = 1;
- bool loopTrajectory = 2;
-
- enum PlanningMode {
- FLUID = 0;
- CARTESIAN = 1;
- }
-
- PlanningMode planningMode = 3;
-}
diff --git a/ros2rag.common/proto/trajectory.proto b/ros2rag.common/proto/trajectory.proto
deleted file mode 100644
index 2724b79c3ba3ed20efa99b6c66e6d415e8f3075d..0000000000000000000000000000000000000000
--- a/ros2rag.common/proto/trajectory.proto
+++ /dev/null
@@ -1,14 +0,0 @@
-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.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
index 352db521149f5d305ad16dc1d5ec6a2091aea2ab..de0758b209fa013132163c178d911124629ad1e3 100644
--- a/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
+++ b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
@@ -1,5 +1,7 @@
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;
}
diff --git a/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/Util.java b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/Util.java
index 471c21abdb1a7cf8c5dc0cd193e9db8d542d1209..7c6d930aefa358a24b53be7349e1d667fc681380 100644
--- a/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/Util.java
+++ b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/Util.java
@@ -1,7 +1,13 @@
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);
diff --git a/ros2rag.common/src/main/proto/config.proto b/ros2rag.common/src/main/proto/config.proto
new file mode 100644
index 0000000000000000000000000000000000000000..38cb5d0a25b02c96dcbdf30f685212ea972feee6
--- /dev/null
+++ b/ros2rag.common/src/main/proto/config.proto
@@ -0,0 +1,7 @@
+syntax = "proto3";
+
+package config;
+
+message RobotConfig {
+ double speed = 1;
+}
\ No newline at end of file
diff --git a/ros2rag.common/src/main/proto/robot_state.proto b/ros2rag.common/src/main/proto/robot_state.proto
new file mode 100644
index 0000000000000000000000000000000000000000..6630631f16e00493be3f50307c9092e56184e9c6
--- /dev/null
+++ b/ros2rag.common/src/main/proto/robot_state.proto
@@ -0,0 +1,37 @@
+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
diff --git a/ros2rag.common/src/main/proto/trajectory.proto b/ros2rag.common/src/main/proto/trajectory.proto
new file mode 100644
index 0000000000000000000000000000000000000000..4a7f375e1ae37ba386f45e1149dc38990d0c7a0e
--- /dev/null
+++ b/ros2rag.common/src/main/proto/trajectory.proto
@@ -0,0 +1,33 @@
+syntax = "proto3";
+
+package plan;
+
+message Trajectory {
+
+ 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;
+ }
+
+ enum PlanningMode {
+ FLUID = 0;
+ CARTESIAN = 1;
+ }
+
+ message Pose {
+ Position position = 1;
+ Orientation orientation = 2;
+ PlanningMode mode = 3;
+ }
+
+ repeated Pose pose = 1;
+ bool loop = 2;
+}
diff --git a/ros2rag.goal/build.gradle b/ros2rag.goal/build.gradle
index c6ee36c57fce4032365deb999b3e72393dd31b9c..c124190b772948fa6bdcff959c4fdc68769d793a 100644
--- a/ros2rag.goal/build.gradle
+++ b/ros2rag.goal/build.gradle
@@ -1,8 +1,5 @@
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'
- }
-}
diff --git a/ros2rag.goal/src/main/jastadd/Computation.jrag b/ros2rag.goal/src/main/jastadd/Computation.jrag
index ffa46d6323e2552f36e1da7442991f1fc8c5da61..da1d21fae8d8891b554c8dec90da3e2d2549a4a9 100644
--- a/ros2rag.goal/src/main/jastadd/Computation.jrag
+++ b/ros2rag.goal/src/main/jastadd/Computation.jrag
@@ -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();
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
index 188b6bfc7c757c3e1553f260ab6062c65bf026df..8c1da3c6a493ea651fc94180bf110b2957cb794d 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
@@ -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 {:
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
index 75db6261c62fb5464484a99baf1f032906f5e3af..b8a4c7c29cb3c35a4d5a026fbacb943754b7d328 100644
--- 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
@@ -1,15 +1,11 @@
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);
diff --git a/ros2rag.goal/src/main/proto b/ros2rag.goal/src/main/proto
deleted file mode 120000
index bb7d2bae29902b994e92987325a878a0fe318c69..0000000000000000000000000000000000000000
--- a/ros2rag.goal/src/main/proto
+++ /dev/null
@@ -1 +0,0 @@
-../../../ros2rag.common/proto/
\ No newline at end of file
diff --git a/ros2rag.receiverstub/build.gradle b/ros2rag.receiverstub/build.gradle
index da0cc2314042c7ed9d58af5f604d41fbfef3679a..db37ca24aa4b861c2153967a3092f38f685a5dfa 100644
--- a/ros2rag.receiverstub/build.gradle
+++ b/ros2rag.receiverstub/build.gradle
@@ -1,7 +1,4 @@
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'
- }
-}
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 8487e621e28b26dd5abc7b78757f797f6f56a411..0ea8aa94ea831952b846cb99bd8451d64ebca12c 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
@@ -1,20 +1,15 @@
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();
+ Trajectory.Orientation orientation = pose.getOrientation();
+ Trajectory.PlanningMode mode = pose.getMode();
+ sb.append(String.format(
+ "pos(% .5f,% .5f,% .5f), ori(%.1f,%.1f,%.1f,%.1f) plan: %s",
+ position.getX(),
+ position.getY(),
+ position.getZ(),
+ orientation.getX(),
+ orientation.getY(),
+ orientation.getZ(),
+ orientation.getW(),
+ mode.name()));
+ if (first) {
+ first = false;
+ } else {
+ sb.append(" | ");
+ }
}
+ sb.append("] loop: ").append(trajectory.getLoop());
logger.info(sb.toString());
}
} catch (InvalidProtocolBufferException e) {
diff --git a/ros2rag.senderstub/build.gradle b/ros2rag.senderstub/build.gradle
index 8a0b44120af2f5f4a907533dafc077c5985622bf..063f22ec23add73ccaaf1c50fbab95b2510c425c 100644
--- a/ros2rag.senderstub/build.gradle
+++ b/ros2rag.senderstub/build.gradle
@@ -1,7 +1,4 @@
apply plugin: 'application'
-apply plugin: 'com.google.protobuf'
-
-sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.senderstub.SenderMain'
@@ -11,31 +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.senderstub.SenderMain')
dependencies {
implementation project(':ros2rag.starter')
-
- protobuf files("$projectDir/../ros2rag.example/src/main/proto")
-}
-
-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'
- }
+ implementation project(':ros2rag.common')
}
diff --git a/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/SenderMain.java b/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/SenderMain.java
index f540f1cc8825fcec1c357c7046ed94e47a24cbe7..e767cdd92c57d713597ddf69c414a0308f24f1e4 100644
--- a/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/SenderMain.java
+++ b/ros2rag.senderstub/src/main/java/de/tudresden/inf/st/ros2rag/senderstub/SenderMain.java
@@ -1,7 +1,7 @@
package de.tudresden.inf.st.ros2rag.senderstub;
import de.tudresden.inf.st.ros2rag.starter.ast.MqttHandler;
-import panda.Linkstate;
+import robot.RobotStateOuterClass.RobotState;
import java.nio.file.Files;
import java.nio.file.Paths;
@@ -47,13 +47,9 @@ public class SenderMain {
}
private static void publish(MqttHandler sender, String topic, float x, float y, float z) {
- Linkstate.PandaLinkState pls = Linkstate.PandaLinkState.newBuilder()
+ RobotState pls = RobotState.newBuilder()
.setName(topic)
- .setPos(Linkstate.PandaLinkState.Position.newBuilder()
- .setPositionX(x)
- .setPositionY(y)
- .setPositionZ(z)
- .build())
+ .setPosition(RobotState.Position.newBuilder().setX(x).setY(y).setZ(z).build())
.build();
final byte[] message = pls.toByteArray();
sender.publish(topic, message);
diff --git a/ros2rag.starter/build.gradle b/ros2rag.starter/build.gradle
index a2e813046f9481e19f079cb31e47a56daff2d199..88f304aa3177f09de185f0bafe0e507e80a2d919 100644
--- a/ros2rag.starter/build.gradle
+++ b/ros2rag.starter/build.gradle
@@ -1,8 +1,5 @@
apply plugin: 'jastadd'
apply plugin: 'application'
-apply plugin: 'com.google.protobuf'
-
-sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.starter.StarterMain'
@@ -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,28 +19,16 @@ configurations {
}
sourceSets.main.java.srcDir "src/gen/java"
-jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.starter.StarterMain')
dependencies {
implementation project (':ros2rag.base')
implementation project (':ros2rag.common')
baseRuntimeClasspath project (':ros2rag.base')
-// implementation group: 'com.fasterxml.jackson.dataformat', name: 'jackson-dataformat-xml', version: "${jackson_version}"
- 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"]
@@ -136,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'
- }
-}
diff --git a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag
index 99aec4416578d7af313846eb2ed33645b58cc358..eff6850999375f3a57e8d74d614da8dd4059da24 100644
--- a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag
+++ b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag
@@ -1,35 +1,31 @@
-/**
- * Version 2020-05-28
- */
+/* Version 2020-07-15 */
// --- update definitions ---
-read Link.CurrentPosition using ParseLinkState, LinkStateToIntPosition ;
+read Link.CurrentPosition using ParseRobotState, RobotStateToIntPosition ;
write RobotArm.AppropriateSpeed using CreateSpeedMessage, SerializeRobotConfig ;
// --- dependency definitions ---
RobotArm.AppropriateSpeed canDependOn Link.CurrentPosition as dependency1 ;
// --- 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);
:}
-SerializeRobotConfig maps config.Robotconfig.RobotConfig rc to byte[] {:
+SerializeRobotConfig maps config.Config.RobotConfig rc to byte[] {:
// System.out.println("SerializeRobotConfig");
return rc.toByteArray();
:}
-LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
-// System.out.println("LinkStateToIntPosition");
- panda.Linkstate.PandaLinkState.Position p = pls.getPos();
- return IntPosition.of((int) (Math.round(p.getPositionX() * 2)), (int) (Math.round(p.getPositionY() * 2)), (int) (Math.round(p.getPositionZ() * 2 - 0.5)));
+RobotStateToIntPosition maps robot.RobotStateOuterClass.RobotState rs to IntPosition {:
+// System.out.println("RobotStateToIntPosition");
+ robot.RobotStateOuterClass.RobotState.Position p = rs.getPosition();
+ return IntPosition.of((int) (Math.round(p.getX() * 2)), (int) (Math.round(p.getY() * 2)), (int) (Math.round(p.getZ() * 2 - 0.5)));
:}
-CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
+CreateSpeedMessage maps double speed to config.Config.RobotConfig {:
// System.out.println("CreateSpeedMessage");
- return config.Robotconfig.RobotConfig.newBuilder()
+ return config.Config.RobotConfig.newBuilder()
.setSpeed(speed)
- .setLoopTrajectory(true)
- .setPlanningMode(config.Robotconfig.RobotConfig.PlanningMode.FLUID)
.build();
:}
diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java
index 63e9ea7ab7f933b0e6c41b8669c43aef195144f9..7627ac005d67c1332323f95866b5f55703cfb532 100644
--- a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java
+++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java
@@ -1,12 +1,8 @@
package de.tudresden.inf.st.ros2rag.starter;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.fasterxml.jackson.dataformat.yaml.YAMLFactory;
-import config.Dataconfig;
+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.*;
-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;
@@ -27,14 +23,11 @@ public class StarterMain {
private Model model;
public void run(String[] args) throws IOException, InterruptedException {
- File configFile = new File(args.length == 0 ? "common/config.yaml" : args[0]);
+ 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;
-
+ ActualConfiguration config = Util.parseConfig(configFile);
model = new Model();
Util.setMqttHost(model::MqttSetHost, config);
@@ -68,7 +61,7 @@ public class StarterMain {
robotArm.addLink(link);
}
link.setName(name);
- link.setCurrentPosition(makePosition(0, 0, 0));
+ link.setCurrentPosition(IntPosition.of(0, 0, 0));
link.containingRobotArm().addDependency1(link);
link.connectCurrentPosition(topic);
}, config);
@@ -87,8 +80,6 @@ public class StarterMain {
mainHandler.newConnection("exit", bytes -> exitCondition.countDown());
mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm));
- sendInitialDataConfig(mainHandler, config.topics.dataConfig);
-
Runtime.getRuntime().addShutdownHook(new Thread(this::close));
exitCondition.await();
@@ -96,17 +87,6 @@ public class StarterMain {
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, RobotArm robotArm) {
StringBuilder sb = new StringBuilder(prefix).append("\n")
.append("robotArm.isInSafetyZone: ").append(robotArm.isInSafetyZone())
@@ -119,10 +99,6 @@ public class StarterMain {
logger.info(sb.toString());
}
- private static IntPosition makePosition(int x, int y, int z) {
- return IntPosition.of(x, y, z);
- }
-
private void close() {
logger.info("Exiting ...");
mainHandler.close();
diff --git a/ros2rag.starter/src/main/proto b/ros2rag.starter/src/main/proto
deleted file mode 120000
index bb7d2bae29902b994e92987325a878a0fe318c69..0000000000000000000000000000000000000000
--- a/ros2rag.starter/src/main/proto
+++ /dev/null
@@ -1 +0,0 @@
-../../../ros2rag.common/proto/
\ No newline at end of file
diff --git a/ros2rag.tests/build.gradle b/ros2rag.tests/build.gradle
index 724c10a6b88a941125b36b08b276d95202fbbc95..efff9a553b1da39904b80933df36e4e4d809f526 100644
--- a/ros2rag.tests/build.gradle
+++ b/ros2rag.tests/build.gradle
@@ -1,7 +1,6 @@
import org.jastadd.relast.plugin.RelastPlugin
import org.jastadd.relast.plugin.RelastTest
apply plugin: 'jastadd'
-apply plugin: 'com.google.protobuf'
apply plugin: RelastPlugin
sourceCompatibility = 1.8
@@ -14,12 +13,12 @@ buildscript {
repositories.jcenter()
dependencies {
classpath 'org.jastadd:jastaddgradle:1.13.3'
- classpath 'com.google.protobuf:protobuf-gradle-plugin:0.8.12'
}
}
dependencies {
implementation project(':ros2rag.base')
+ implementation project(':ros2rag.common')
runtime group: 'org.jastadd', name: 'jastadd', version: '2.3.4'
testImplementation group: 'org.junit.jupiter', name: 'junit-jupiter-api', version: '5.4.0'
@@ -27,7 +26,6 @@ dependencies {
testImplementation group: 'org.assertj', name: 'assertj-core', version: '3.12.1'
testImplementation group: 'org.fusesource.mqtt-client', name: 'mqtt-client', version: '1.15'
testImplementation group: 'net.sf.beaver', name: 'beaver-rt', version: '0.9.11'
- testImplementation group: 'com.google.protobuf', name: 'protobuf-java', version: '3.0.0'
}
test {
@@ -57,15 +55,6 @@ sourceSets {
}
}
-protobuf {
- // create strange directories, so use default here
- generatedFilesBaseDir = "$projectDir/src/test/java-gen/proto"
- protoc {
- // The artifact spec for the Protobuf Compiler
- artifact = 'com.google.protobuf:protoc:3.0.0'
- }
-}
-
// --- Test: Example ---
task preprocessExampleTest(type: JavaExec, group: 'verification') {
doFirst {
@@ -221,5 +210,5 @@ testClasses.dependsOn compileRead2Write1Test
compileRead2Write1Test.dependsOn preprocessRead2Write1Test
clean {
- delete 'src/test/02-after-ros2rag/*/', 'src/test/03-after-relast/*/'
+ delete 'src/test/02-after-ros2rag/*/', 'src/test/03-after-relast/*/', 'src/test/java-gen/*/'
}
diff --git a/ros2rag.tests/src/test/01-input/example/Test.jadd b/ros2rag.tests/src/test/01-input/example/Test.jadd
index c355f57814c9def67657a148a5a28061e2569d28..b62b9aee7cb344c587993feefccdd64ab0bf6e4b 100644
--- a/ros2rag.tests/src/test/01-input/example/Test.jadd
+++ b/ros2rag.tests/src/test/01-input/example/Test.jadd
@@ -60,14 +60,14 @@ aspect GrammarTypes {
inh Model RobotArm.model();
eq Model.getRobotArm().model() = this;
- inh RobotArm Joint.containingRobotArm();
- eq RobotArm.getJoint().containingRobotArm() = this;
+ inh RobotArm Link.containingRobotArm();
+ eq RobotArm.getLink().containingRobotArm() = this;
eq RobotArm.getEndEffector().containingRobotArm() = this;
syn boolean RobotArm.isInSafetyZone() {
TestCounter.INSTANCE.numberInSafetyZone += 1;
- for (Joint joint : getJointList()) {
- if (model().getZoneModel().isInSafetyZone(joint.getCurrentPosition())) {
+ for (Link link : getLinkList()) {
+ if (model().getZoneModel().isInSafetyZone(link.getCurrentPosition())) {
return true;
}
}
@@ -78,13 +78,7 @@ aspect GrammarTypes {
syn boolean ZoneModel.isInSafetyZone(IntPosition pos) {
for (Zone sz : getSafetyZoneList()) {
for (Coordinate coordinate : sz.getCoordinateList()) {
- IntPosition inside = coordinate.getPosition();
- if (inside.getX() <= pos.getX() &&
- inside.getY() <= pos.getY() &&
- inside.getZ() <= pos.getZ() &&
- pos.getX() <= inside.getX() + getSize().getX() &&
- pos.getY() <= inside.getY() + getSize().getY() &&
- pos.getZ() <= inside.getZ() + getSize().getZ()) {
+ if (coordinate.getPosition().equals(pos)) {
return true;
}
}
diff --git a/ros2rag.tests/src/test/01-input/example/Test.relast b/ros2rag.tests/src/test/01-input/example/Test.relast
index ea347d0ec303a8d3555cd0118d403b53664ffe33..b8d932ae470e4bf3dc433f8610eec1f05016e595 100644
--- a/ros2rag.tests/src/test/01-input/example/Test.relast
+++ b/ros2rag.tests/src/test/01-input/example/Test.relast
@@ -1,13 +1,13 @@
Model ::= RobotArm ZoneModel ;
-ZoneModel ::= <Size:IntPosition> SafetyZone:Zone* ;
+ZoneModel ::= SafetyZone:Zone* ;
Zone ::= Coordinate* ;
-RobotArm ::= Joint* EndEffector /<AppropriateSpeed:double>/ ;
+RobotArm ::= Link* EndEffector /<AppropriateSpeed:double>/ ;
-Joint ::= <Name:String> <CurrentPosition:IntPosition> ;
+Link ::= <Name:String> <CurrentPosition:IntPosition> ;
-EndEffector : Joint;
+EndEffector : Link;
Coordinate ::= <Position:IntPosition> ;
diff --git a/ros2rag.tests/src/test/01-input/example/Test.ros2rag b/ros2rag.tests/src/test/01-input/example/Test.ros2rag
index 5c929386c01052f3e9cfc2bcd81fe45482ea8446..c916bfdca59f605ed090fa1171dc5e068fe23a8f 100644
--- a/ros2rag.tests/src/test/01-input/example/Test.ros2rag
+++ b/ros2rag.tests/src/test/01-input/example/Test.ros2rag
@@ -1,31 +1,31 @@
-/* Version 2020-04-17 */
+/* Version 2020-07-15 */
// --- update definitions ---
-read Joint.CurrentPosition using ParseLinkState, LinkStateToIntPosition ;
+read Link.CurrentPosition using ParseRobotState, RobotStateToIntPosition ;
write RobotArm.AppropriateSpeed using CreateSpeedMessage, SerializeRobotConfig ;
// --- dependency definitions ---
-RobotArm.AppropriateSpeed canDependOn Joint.CurrentPosition as dependency1 ;
+RobotArm.AppropriateSpeed canDependOn Link.CurrentPosition as dependency1 ;
// --- mapping definitions ---
-ParseLinkState maps byte[] bytes to panda.Linkstate.PandaLinkState {:
+ParseRobotState maps byte[] bytes to robot.RobotStateOuterClass.RobotState {:
TestCounter.INSTANCE.numberParseLinkState += 1;
- return panda.Linkstate.PandaLinkState.parseFrom(bytes);
+ return robot.RobotStateOuterClass.RobotState.parseFrom(bytes);
:}
-SerializeRobotConfig maps config.Robotconfig.RobotConfig rc to byte[] {:
+SerializeRobotConfig maps config.Config.RobotConfig rc to byte[] {:
TestCounter.INSTANCE.numberSerializeRobotConfig += 1;
return rc.toByteArray();
:}
-LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
+RobotStateToIntPosition maps robot.RobotStateOuterClass.RobotState rs to IntPosition {:
TestCounter.INSTANCE.numberLinkStateToIntPosition += 1;
- panda.Linkstate.PandaLinkState.Position p = pls.getPos();
- return IntPosition.of((int) (10 * p.getPositionX()), (int) (10 * p.getPositionY()), (int) (10 * p.getPositionZ()));
+ robot.RobotStateOuterClass.RobotState.Position p = rs.getPosition();
+ return IntPosition.of((int) (Math.round(p.getX() * 10)), (int) (Math.round(p.getY() * 10)), (int) (Math.round(p.getZ() * 10)));
:}
-CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
+CreateSpeedMessage maps double speed to config.Config.RobotConfig {:
TestCounter.INSTANCE.numberCreateSpeedMessage += 1;
- return config.Robotconfig.RobotConfig.newBuilder()
+ return config.Config.RobotConfig.newBuilder()
.setSpeed(speed)
.build();
:}
diff --git a/ros2rag.tests/src/test/java/org/jastadd/ros2rag/tests/ExampleTest.java b/ros2rag.tests/src/test/java/org/jastadd/ros2rag/tests/ExampleTest.java
index 0315358c9afade8d98b8b09782913ee985d85f2d..9b1ebceb541bddd4cafc3648b10c940af79bc952 100644
--- a/ros2rag.tests/src/test/java/org/jastadd/ros2rag/tests/ExampleTest.java
+++ b/ros2rag.tests/src/test/java/org/jastadd/ros2rag/tests/ExampleTest.java
@@ -1,12 +1,12 @@
package org.jastadd.ros2rag.tests;
import com.google.protobuf.InvalidProtocolBufferException;
-import config.Robotconfig.RobotConfig;
+import config.Config.RobotConfig;
import example.ast.*;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
-import panda.Linkstate;
+import robot.RobotStateOuterClass.RobotState;
import java.io.IOException;
import java.util.concurrent.TimeUnit;
@@ -26,8 +26,8 @@ public class ExampleTest extends AbstractMqttTest {
private Model model;
private RobotArm robotArm;
- private Joint joint1;
- private Joint joint2;
+ private Link link1;
+ private Link link2;
private MqttHandler handler;
private ReceiverData data;
@@ -72,7 +72,7 @@ public class ExampleTest extends AbstractMqttTest {
// still in safety zone, so no update should have been sent
TestUtils.waitForMqtt();
- assertEquals(makePosition(2, 2, 2), joint1.getCurrentPosition());
+ assertEquals(makePosition(2, 2, 2), link1.getCurrentPosition());
assertEquals(1, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(1, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(2, TestCounter.INSTANCE.numberInSafetyZone);
@@ -86,7 +86,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 0.3f, 0.4f, 0.5f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(3, 4, 5), joint2.getCurrentPosition());
+ assertEquals(makePosition(3, 4, 5), link2.getCurrentPosition());
assertEquals(2, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(2, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(3, TestCounter.INSTANCE.numberInSafetyZone);
@@ -100,7 +100,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 0.33f, 0.42f, 0.51f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(3, 4, 5), joint2.getCurrentPosition());
+ assertEquals(makePosition(3, 4, 5), link2.getCurrentPosition());
assertEquals(3, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(3, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(3, TestCounter.INSTANCE.numberInSafetyZone);
@@ -113,7 +113,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 1.3f, 2.4f, 3.5f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(13, 24, 35), joint2.getCurrentPosition());
+ assertEquals(makePosition(13, 24, 35), link2.getCurrentPosition());
assertEquals(4, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(4, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(4, TestCounter.INSTANCE.numberInSafetyZone);
@@ -142,7 +142,7 @@ public class ExampleTest extends AbstractMqttTest {
// still in safety zone, hence, no value should have been sent
TestUtils.waitForMqtt();
- assertEquals(makePosition(2, 2, 2), joint1.getCurrentPosition());
+ assertEquals(makePosition(2, 2, 2), link1.getCurrentPosition());
assertEquals(1, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(1, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(2, TestCounter.INSTANCE.numberInSafetyZone);
@@ -154,7 +154,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 0.3f, 0.4f, 0.5f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(3, 4, 5), joint2.getCurrentPosition());
+ assertEquals(makePosition(3, 4, 5), link2.getCurrentPosition());
assertEquals(2, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(2, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(3, TestCounter.INSTANCE.numberInSafetyZone);
@@ -168,7 +168,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 0.33f, 0.42f, 0.51f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(3, 4, 5), joint2.getCurrentPosition());
+ assertEquals(makePosition(3, 4, 5), link2.getCurrentPosition());
assertEquals(3, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(3, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(3, TestCounter.INSTANCE.numberInSafetyZone);
@@ -181,7 +181,7 @@ public class ExampleTest extends AbstractMqttTest {
sendData(TOPIC_JOINT2, 1.3f, 2.4f, 3.5f);
TestUtils.waitForMqtt();
- assertEquals(makePosition(13, 24, 35), joint2.getCurrentPosition());
+ assertEquals(makePosition(13, 24, 35), link2.getCurrentPosition());
assertEquals(4, TestCounter.INSTANCE.numberParseLinkState);
assertEquals(4, TestCounter.INSTANCE.numberLinkStateToIntPosition);
assertEquals(4, TestCounter.INSTANCE.numberInSafetyZone);
@@ -202,12 +202,8 @@ public class ExampleTest extends AbstractMqttTest {
}
private void sendData(String topic, float x, float y, float z) {
- handler.publish(topic, Linkstate.PandaLinkState.newBuilder()
- .setPos(Linkstate.PandaLinkState.Position.newBuilder()
- .setPositionX(x)
- .setPositionY(y)
- .setPositionZ(z)
- .build())
+ handler.publish(topic, RobotState.newBuilder()
+ .setPosition(RobotState.Position.newBuilder().setX(x).setY(y).setZ(z).build())
.build()
.toByteArray()
);
@@ -221,8 +217,8 @@ public class ExampleTest extends AbstractMqttTest {
assertTrue(handler.waitUntilReady(2, TimeUnit.SECONDS));
// add dependencies
- robotArm.addDependency1(joint1);
- robotArm.addDependency1(joint2);
+ robotArm.addDependency1(link1);
+ robotArm.addDependency1(link2);
robotArm.addDependency1(robotArm.getEndEffector());
data = new ReceiverData();
@@ -238,15 +234,14 @@ public class ExampleTest extends AbstractMqttTest {
});
robotArm.connectAppropriateSpeed(TOPIC_CONFIG, writeCurrentValue);
- joint1.connectCurrentPosition(TOPIC_JOINT1);
- joint2.connectCurrentPosition(TOPIC_JOINT2);
+ link1.connectCurrentPosition(TOPIC_JOINT1);
+ link2.connectCurrentPosition(TOPIC_JOINT2);
}
private void createModel() {
model = new Model();
ZoneModel zoneModel = new ZoneModel();
- zoneModel.setSize(makePosition(1, 1, 1));
IntPosition firstPosition = makePosition(0, 0, 0);
IntPosition secondPosition = makePosition(-1, 0, 0);
@@ -261,20 +256,20 @@ public class ExampleTest extends AbstractMqttTest {
robotArm = new RobotArm();
- joint1 = new Joint();
- joint1.setName("joint1");
- joint1.setCurrentPosition(firstPosition);
+ link1 = new Link();
+ link1.setName("joint1");
+ link1.setCurrentPosition(firstPosition);
- joint2 = new Joint();
- joint2.setName("joint2");
- joint2.setCurrentPosition(secondPosition);
+ link2 = new Link();
+ link2.setName("joint2");
+ link2.setCurrentPosition(secondPosition);
EndEffector endEffector = new EndEffector();
endEffector.setName("gripper");
endEffector.setCurrentPosition(makePosition(2, 2, 3));
- robotArm.addJoint(joint1);
- robotArm.addJoint(joint2);
+ robotArm.addLink(link1);
+ robotArm.addLink(link2);
robotArm.setEndEffector(endEffector);
model.setRobotArm(robotArm);
}