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

Use JSON config for setup (both starter and receiver).

- Starter: Send initial DataConfig
- Starter: Always send complete RobotConfig
parent bd6c3376
Pipeline #6886 failed with stage
in 2 minutes and 56 seconds
...@@ -7,6 +7,7 @@ import org.jastadd.ros2rag.scanner.Ros2RagScanner; ...@@ -7,6 +7,7 @@ import org.jastadd.ros2rag.scanner.Ros2RagScanner;
import java.io.BufferedReader; import java.io.BufferedReader;
import java.io.IOException; import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.file.Files; import java.nio.file.Files;
import java.nio.file.Path; import java.nio.file.Path;
import java.nio.file.Paths; import java.nio.file.Paths;
...@@ -17,34 +18,95 @@ import java.nio.file.Paths; ...@@ -17,34 +18,95 @@ import java.nio.file.Paths;
* @author rschoene - Initial contribution * @author rschoene - Initial contribution
*/ */
public class SimpleMain { public class SimpleMain {
// --- just testing byte[] conversion ---
public static void testing() {
byte[] bytes;
int i = 1;
double d = 2.3d;
float f = 4.2f;
short sh = 13;
long l = 7L;
String s = "Hello";
char c = 'a';
Integer ii = Integer.valueOf(1);
if (!ii.equals(i)) throw new AssertionError("Ints not equal");
// int to byte
ByteBuffer i2b = ByteBuffer.allocate(4);
i2b.putInt(i);
bytes = i2b.array();
// byte to int
ByteBuffer b2i = ByteBuffer.wrap(bytes);
int actual_i = b2i.getInt();
if (i != actual_i) throw new AssertionError("Ints not equal");
// double to byte
ByteBuffer d2b = ByteBuffer.allocate(8);
d2b.putDouble(d);
bytes = d2b.array();
// byte to double
ByteBuffer b2d = ByteBuffer.wrap(bytes);
double actual_d = b2d.getDouble();
if (d != actual_d) throw new AssertionError("Doubles not equal");
// float to byte
ByteBuffer f2b = ByteBuffer.allocate(4);
f2b.putFloat(f);
bytes = f2b.array();
// byte to float
ByteBuffer b2f = ByteBuffer.wrap(bytes);
float actual_f = b2f.getFloat();
if (f != actual_f) throw new AssertionError("Floats not equal");
// short to byte
ByteBuffer sh2b = ByteBuffer.allocate(2);
sh2b.putShort(sh);
bytes = sh2b.array();
// byte to short
ByteBuffer b2sh = ByteBuffer.wrap(bytes);
short actual_sh = b2sh.getShort();
if (sh != actual_sh) throw new AssertionError("Shorts not equal");
// long to byte
ByteBuffer l2b = ByteBuffer.allocate(8);
l2b.putLong(l);
bytes = l2b.array();
// byte to long
ByteBuffer b2l = ByteBuffer.wrap(bytes);
long actual_l = b2l.getLong();
if (l != actual_l) throw new AssertionError("Longs not equal");
// String to byte
bytes = s.getBytes();
// byte to String
String actual_s = new String(bytes);
if (!s.equals(actual_s)) throw new AssertionError("Strings not equal");
// char to byte
ByteBuffer c2b = ByteBuffer.allocate(2);
c2b.putChar(c);
bytes = c2b.array();
// byte to char
ByteBuffer b2c = ByteBuffer.wrap(bytes);
char actual_c = b2c.getChar();
if (c != actual_c) throw new AssertionError("Floats not equal");
}
public static void main(String[] args) { public static void main(String[] args) {
/* testing();
// as soon as the cache of isInSafetyZone is invalidated, update the value of Robot.ShouldUseLowSpeed with its value // createManualAST();
[always] update Robot.ShouldUseLowSpeed with isInSafetyZone() using transformation(); }
// when a (new?) value for ShouldUseLowSpeed is set, send it over via mqtt private static void createManualAST() {
[always] write Robot.ShouldUseLowSpeed;
// when an update of pose is read via mqtt, then update current position
[always] read Joint.CurrentPosition using PoseToPosition;
// PBPose is a datatype defined in protobuf
PoseToPosition: map PBPose to Position using
pose.position.x += sqrt(.5 * size.x)
MAP round(2)
x = x / 100
IGNORE_IF_SAME
;
--- using generated methods ---
Joint j;
j.getCurrentPosition().connectTo("/robot/joint2/pos");
RobotArm r;
// this should not be required
r.getShouldUseLowSpeed().addObserver(j.getCurrentPosition());
r.getShouldUseLowSpeed().connectTo("/robot/config/speed");
*/
Ros2Rag model = new Ros2Rag(); Ros2Rag model = new Ros2Rag();
Program program = parseProgram(Paths.get("src", "test", "resources", "Example.relast")); Program program = parseProgram(Paths.get("src", "test", "resources", "Example.relast"));
model.setProgram(program); model.setProgram(program);
......
package de.tudresden.inf.st.ros2rag.receiverstub; package de.tudresden.inf.st.ros2rag.receiverstub;
import com.beust.jcommander.JCommander;
import com.beust.jcommander.Parameter;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.google.protobuf.InvalidProtocolBufferException; import com.google.protobuf.InvalidProtocolBufferException;
import config.Dataconfig;
import config.Dataconfig.DataConfig;
import config.Robotconfig.RobotConfig; import config.Robotconfig.RobotConfig;
import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater; import de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater;
import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration;
import de.tudresden.inf.st.ros2rag.starter.data.DataJoint;
import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger; import org.apache.logging.log4j.Logger;
import panda.Linkstate.PandaLinkState; import panda.Linkstate.PandaLinkState;
import java.io.File;
import java.io.IOException;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
import java.util.concurrent.locks.Condition; import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.Lock;
...@@ -17,83 +30,103 @@ public class Main { ...@@ -17,83 +30,103 @@ public class Main {
private static final Logger logger = LogManager.getLogger(Main.class); private static final Logger logger = LogManager.getLogger(Main.class);
public static void main(String[] args) throws Exception { public static void main(String[] args) throws Exception {
String jointTopic, configTopic; Main main = new Main();
if (args.length < 2) { ObjectMapper mapper = new ObjectMapper();
jointTopic = "robot/joint1"; File configFile = new File(args[0]);
configTopic = "robot/config"; System.out.println("Using config file: " + configFile.getAbsolutePath());
} else { DataConfiguration config = mapper.readValue(configFile, DataConfiguration.class);
jointTopic = args[0]; main.run(config);
configTopic = args[1]; }
}
final Lock finishLock = new ReentrantLock(); private void run(DataConfiguration config) throws IOException, InterruptedException {
final Condition finishCondition = finishLock.newCondition(); final CountDownLatch finish = new CountDownLatch(1);
MqttUpdater receiver = new MqttUpdater("receiver stub"); MqttUpdater receiver = new MqttUpdater("receiver stub");
receiver.setHost("localhost", 1883); receiver.setHost(config.mqttHost);
receiver.waitUntilReady(2, TimeUnit.SECONDS); receiver.waitUntilReady(2, TimeUnit.SECONDS);
receiver.newConnection(configTopic, bytes -> { receiver.newConnection(config.robotConfigTopic, this::printRobotConfig);
try { receiver.newConnection(config.dataConfigTopic, this::printDataConfig);
logger.debug("Got a config message, parsing ..."); for (DataJoint joint : config.joints) {
RobotConfig robotConfig = RobotConfig.parseFrom(bytes); receiver.newConnection(joint.topic, this::printPandaLinkState);
logger.info("robotConfig: speed = {}, loopTrajectory = {}, planningMode = {}", }
robotConfig.getSpeed(),
robotConfig.getLoopTrajectory(),
robotConfig.getPlanningMode().toString());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
});
receiver.newConnection(jointTopic, 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();
logger.info("{}: pos({},{},{}), orient({},{},{},{})," +
" twist-linear({},{},{}), twist-angular({},{},{})",
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());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
});
receiver.newConnection("components", bytes -> { receiver.newConnection("components", bytes -> {
String message = new String(bytes); String message = new String(bytes);
logger.info("Components: {}", message); logger.info("Components: {}", message);
}); });
receiver.newConnection("receiver", bytes -> { receiver.newConnection("receiver", bytes -> {
String message = new String(bytes); String message = new String(bytes);
if (message.equals("exit")) { if (message.equals("exit")) {
try { finish.countDown();
finishLock.lock();
finishCondition.signal();
} finally {
finishLock.unlock();
}
} }
}); });
try {
finishLock.lock(); Runtime.getRuntime().addShutdownHook(new Thread(receiver::close));
finishCondition.await(); if (config.exitAfterSeconds > 0) {
} finally { finish.await(config.exitAfterSeconds, TimeUnit.SECONDS);
finishLock.unlock(); } else {
finish.await();
} }
receiver.close(); receiver.close();
Runtime.getRuntime().addShutdownHook(new Thread(receiver::close));
} }
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();
logger.info("{}: pos({},{},{}), orient({},{},{},{})," +
" twist-linear({},{},{}), twist-angular({},{},{})",
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());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
}
private void printRobotConfig(byte[] bytes) {
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());
} catch (InvalidProtocolBufferException e) {
e.printStackTrace();
}
}
} }
...@@ -4,7 +4,7 @@ apply plugin: 'com.google.protobuf' ...@@ -4,7 +4,7 @@ apply plugin: 'com.google.protobuf'
sourceCompatibility = 1.8 sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.starter.Main' mainClassName = 'de.tudresden.inf.st.ros2rag.starter.StarterMain'
repositories { repositories {
jcenter() jcenter()
...@@ -23,7 +23,7 @@ configurations { ...@@ -23,7 +23,7 @@ configurations {
} }
sourceSets.main.java.srcDir "src/gen/java" sourceSets.main.java.srcDir "src/gen/java"
jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.starter.Main') jar.manifest.attributes('Main-Class': 'de.tudresden.inf.st.ros2rag.starter.StarterMain')
dependencies { dependencies {
implementation project (':ros2rag.base') implementation project (':ros2rag.base')
......
...@@ -26,5 +26,7 @@ LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {: ...@@ -26,5 +26,7 @@ LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {: CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
return config.Robotconfig.RobotConfig.newBuilder() return config.Robotconfig.RobotConfig.newBuilder()
.setSpeed(speed) .setSpeed(speed)
.setLoopTrajectory(true)
.setPlanningMode(config.Robotconfig.RobotConfig.PlanningMode.CARTESIAN)
.build(); .build();
:} :}
package de.tudresden.inf.st.ros2rag.starter; package de.tudresden.inf.st.ros2rag.starter;
import com.fasterxml.jackson.databind.ObjectMapper;
import config.Dataconfig;
import de.tudresden.inf.st.ros2rag.starter.ast.*; import de.tudresden.inf.st.ros2rag.starter.ast.*;
import de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration;
import de.tudresden.inf.st.ros2rag.starter.data.DataJoint;
import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger; import org.apache.logging.log4j.Logger;
import java.io.File;
import java.io.IOException; import java.io.IOException;
import java.util.concurrent.CountDownLatch; import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
...@@ -13,47 +18,29 @@ import java.util.concurrent.TimeUnit; ...@@ -13,47 +18,29 @@ import java.util.concurrent.TimeUnit;
* *
* @author rschoene - Initial contribution * @author rschoene - Initial contribution
*/ */
public class Main { public class StarterMain {
private static final Logger logger = LogManager.getLogger(Main.class); private static final Logger logger = LogManager.getLogger(StarterMain.class);
private MqttUpdater mainHandler;
private Model model;
public static void main(String[] args) throws IOException, InterruptedException { public void run(String[] args) throws IOException, InterruptedException {
// parse arguments File configFile = new File(args[0]);
final String mqttHost;
if (args.length > 0) {
mqttHost = args[0];
} else {
mqttHost = "localhost";
}
/* assumed configuration:
panda_ground: ground-plate
panda_link_0: link_0 of the panda
panda_link_1: link_1 of the panda
panda_link_2: link_2 of the panda
panda_link_3: link_3 of the panda
panda_link_4: link_4 of the panda
panda_link_5: link_5 of the panda
panda_link_6: link_6 of the panda
panda_link_7: link_7 of the panda (end effector)
panda_link_8: link_8 of the panda (left finger)
panda_link_9: link_9 of the panda (right finger)
*/
final int numberOfJoints = 10;
final int endEffectorIndex = 7;
final String jointTopicFormat = "panda_link_%d";
final String configTopic = "robotconfig";
final int[][] safetyZoneCoordinates = { final int[][] safetyZoneCoordinates = {
{ 0, 0, 0}, {0, 0, 0},
{-1, 0, 0}, {-1, 0, 0},
{ 1, 0, 0} {1, 0, 0}
}; };
// --- No configuration below this line --- // --- No configuration below this line ---
Model model = new Model(); ObjectMapper mapper = new ObjectMapper();
model.MqttSetHost(mqttHost); System.out.println("Using config file: " + configFile.getAbsolutePath());
DataConfiguration config = mapper.readValue(configFile, DataConfiguration.class);
model = new Model();
model.MqttSetHost(config.mqttHost);
ZoneModel zoneModel = new ZoneModel(); ZoneModel zoneModel = new ZoneModel();
zoneModel.setSize(makePosition(1, 1, 1)); zoneModel.setSize(makePosition(1, 1, 1));
...@@ -69,45 +56,58 @@ public class Main { ...@@ -69,45 +56,58 @@ public class Main {
RobotArm robotArm = new RobotArm(); RobotArm robotArm = new RobotArm();
model.setRobotArm(robotArm); model.setRobotArm(robotArm);
for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) { for (DataJoint dataJoint : config.joints) {
final Joint jointOrEndEffector; final Joint jointOrEndEffector;
if (jointIndex == endEffectorIndex) { if (dataJoint.isEndEffector) {
EndEffector endEffector = new EndEffector(); EndEffector endEffector = new EndEffector();
endEffector.setName("gripper");
robotArm.setEndEffector(endEffector); robotArm.setEndEffector(endEffector);
jointOrEndEffector = endEffector; jointOrEndEffector = endEffector;
} else { } else {
Joint joint = new Joint(); Joint joint = new Joint();
joint.setName("joint" + jointIndex);
robotArm.addJoint(joint); robotArm.addJoint(joint);
jointOrEndEffector = joint; jointOrEndEffector = joint;
} }
jointOrEndEffector.setName(dataJoint.name);
jointOrEndEffector.setCurrentPosition(makePosition(0, 0, 0)); jointOrEndEffector.setCurrentPosition(makePosition(0, 0, 0));
robotArm.addDependency1(jointOrEndEffector); robotArm.addDependency1(jointOrEndEffector);
jointOrEndEffector.connectCurrentPosition(String.format(jointTopicFormat, jointIndex)); jointOrEndEffector.connectCurrentPosition(dataJoint.topic);
} }
robotArm.connectAppropriateSpeed(configTopic, true); robotArm.connectAppropriateSpeed(config.robotConfigTopic, true);
logStatus("Start", robotArm); logStatus("Start", robotArm);
CountDownLatch exitCondition = new CountDownLatch(1); CountDownLatch exitCondition = new CountDownLatch(1);
logger.info("To print the current model states, send a message to the topic 'model'."); 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'."); logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C.");
MqttUpdater mainHandler = new MqttUpdater("mainHandler"); mainHandler = new MqttUpdater("mainHandler");
mainHandler.setHost(mqttHost, 1883); mainHandler.setHost(config.mqttHost);
mainHandler.waitUntilReady(2, TimeUnit.SECONDS); mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
mainHandler.newConnection("exit", bytes -> exitCondition.countDown()); mainHandler.newConnection("exit", bytes -> exitCondition.countDown());
mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm)); mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm));
sendInitialDataConfig(mainHandler, config.dataConfigTopic);
Runtime.getRuntime().addShutdownHook(new Thread(this::close));
exitCondition.await(); exitCondition.await();
mainHandler.close(); this.close();
model.MqttCloseConnections();
} }
private static void logStatus(String prefix, RobotArm robotArm) { private void sendInitialDataConfig(MqttUpdater mainHandler, String dataConfigTopic) {
Dataconfig.DataConfig dataConfig = Dataconfig.DataConfig.newBuilder()
.setEnablePosition(true)
.setEnableOrientation(false)
.setEnableTwistAngular(false)
.setEnableTwistLinear(false)
.setPublishRate(200)
.build();