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;
import java.io.BufferedReader;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
......@@ -17,34 +18,95 @@ import java.nio.file.Paths;
* @author rschoene - Initial contribution
*/
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) {
/*
// as soon as the cache of isInSafetyZone is invalidated, update the value of Robot.ShouldUseLowSpeed with its value
[always] update Robot.ShouldUseLowSpeed with isInSafetyZone() using transformation();
// when a (new?) value for ShouldUseLowSpeed is set, send it over via mqtt
[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");
*/
testing();
// createManualAST();
}
private static void createManualAST() {
Ros2Rag model = new Ros2Rag();
Program program = parseProgram(Paths.get("src", "test", "resources", "Example.relast"));
model.setProgram(program);
......
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 config.Dataconfig;
import config.Dataconfig.DataConfig;
import config.Robotconfig.RobotConfig;
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.Logger;
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.locks.Condition;
import java.util.concurrent.locks.Lock;
......@@ -17,83 +30,103 @@ public class Main {
private static final Logger logger = LogManager.getLogger(Main.class);
public static void main(String[] args) throws Exception {
String jointTopic, configTopic;
if (args.length < 2) {
jointTopic = "robot/joint1";
configTopic = "robot/config";
} else {
jointTopic = args[0];
configTopic = args[1];
}
Main main = new Main();
ObjectMapper mapper = new ObjectMapper();
File configFile = new File(args[0]);
System.out.println("Using config file: " + configFile.getAbsolutePath());
DataConfiguration config = mapper.readValue(configFile, DataConfiguration.class);
main.run(config);
}
final Lock finishLock = new ReentrantLock();
final Condition finishCondition = finishLock.newCondition();
private void run(DataConfiguration config) throws IOException, InterruptedException {
final CountDownLatch finish = new CountDownLatch(1);
MqttUpdater receiver = new MqttUpdater("receiver stub");
receiver.setHost("localhost", 1883);
receiver.setHost(config.mqttHost);
receiver.waitUntilReady(2, TimeUnit.SECONDS);
receiver.newConnection(configTopic, bytes -> {
try {
logger.debug("Got a config 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();
}
});
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(config.robotConfigTopic, this::printRobotConfig);
receiver.newConnection(config.dataConfigTopic, this::printDataConfig);
for (DataJoint joint : config.joints) {
receiver.newConnection(joint.topic, this::printPandaLinkState);
}
receiver.newConnection("components", bytes -> {
String message = new String(bytes);
logger.info("Components: {}", message);
});
receiver.newConnection("receiver", bytes -> {
String message = new String(bytes);
if (message.equals("exit")) {
try {
finishLock.lock();
finishCondition.signal();
} finally {
finishLock.unlock();
}
finish.countDown();
}
});
try {
finishLock.lock();
finishCondition.await();
} finally {
finishLock.unlock();
Runtime.getRuntime().addShutdownHook(new Thread(receiver::close));
if (config.exitAfterSeconds > 0) {
finish.await(config.exitAfterSeconds, TimeUnit.SECONDS);
} else {
finish.await();
}
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'
sourceCompatibility = 1.8
mainClassName = 'de.tudresden.inf.st.ros2rag.starter.Main'
mainClassName = 'de.tudresden.inf.st.ros2rag.starter.StarterMain'
repositories {
jcenter()
......@@ -23,7 +23,7 @@ configurations {
}
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 {
implementation project (':ros2rag.base')
......
......@@ -26,5 +26,7 @@ LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
return config.Robotconfig.RobotConfig.newBuilder()
.setSpeed(speed)
.setLoopTrajectory(true)
.setPlanningMode(config.Robotconfig.RobotConfig.PlanningMode.CARTESIAN)
.build();
:}
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.data.DataConfiguration;
import de.tudresden.inf.st.ros2rag.starter.data.DataJoint;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import java.io.File;
import java.io.IOException;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit;
......@@ -13,47 +18,29 @@ import java.util.concurrent.TimeUnit;
*
* @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 {
// parse arguments
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";
public void run(String[] args) throws IOException, InterruptedException {
File configFile = new File(args[0]);
final int[][] safetyZoneCoordinates = {
{ 0, 0, 0},
{0, 0, 0},
{-1, 0, 0},
{ 1, 0, 0}
{1, 0, 0}
};
// --- No configuration below this line ---
Model model = new Model();
model.MqttSetHost(mqttHost);
ObjectMapper mapper = new ObjectMapper();
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.setSize(makePosition(1, 1, 1));
......@@ -69,45 +56,58 @@ public class Main {
RobotArm robotArm = new RobotArm();
model.setRobotArm(robotArm);
for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++) {
for (DataJoint dataJoint : config.joints) {
final Joint jointOrEndEffector;
if (jointIndex == endEffectorIndex) {
if (dataJoint.isEndEffector) {
EndEffector endEffector = new EndEffector();
endEffector.setName("gripper");
robotArm.setEndEffector(endEffector);
jointOrEndEffector = endEffector;
} else {
Joint joint = new Joint();
joint.setName("joint" + jointIndex);
robotArm.addJoint(joint);
jointOrEndEffector = joint;
}
jointOrEndEffector.setName(dataJoint.name);
jointOrEndEffector.setCurrentPosition(makePosition(0, 0, 0));
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);
CountDownLatch exitCondition = new CountDownLatch(1);
logger.info("To print the current model states, send a message to the topic 'model'.");
logger.info("To exit the system cleanly, send a message to the topic 'exit'.");
logger.info("To exit the system cleanly, send a message to the topic 'exit', or use Ctrl+C.");
MqttUpdater mainHandler = new MqttUpdater("mainHandler");
mainHandler.setHost(mqttHost, 1883);
mainHandler = new MqttUpdater("mainHandler");
mainHandler.setHost(config.mqttHost);
mainHandler.waitUntilReady(2, TimeUnit.SECONDS);
mainHandler.newConnection("exit", bytes -> exitCondition.countDown());
mainHandler.newConnection("model", bytes -> logStatus(new String(bytes), robotArm));
sendInitialDataConfig(mainHandler, config.dataConfigTopic);
Runtime.getRuntime().addShutdownHook(new Thread(this::close));
exitCondition.await();
mainHandler.close();
model.MqttCloseConnections();
this.close();
}
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();
mainHandler.publish(dataConfigTopic, dataConfig.toByteArray());
}
private void logStatus(String prefix, RobotArm robotArm) {
StringBuilder sb = new StringBuilder(prefix).append("\n")
.append("robotArm.isInSafetyZone: ").append(robotArm.isInSafetyZone())
.append(", robotArm.getAppropriateSpeed = ").append(robotArm.getAppropriateSpeed()).append("\n");
......@@ -122,4 +122,14 @@ public class Main {
private static IntPosition makePosition(int x, int y, int z) {
return IntPosition.of(x, y, z);
}
private void close() {
logger.info("Exiting ...");
mainHandler.close();
model.MqttCloseConnections();
}
public static void main(String[] args) throws IOException, InterruptedException {
new StarterMain().run(args);
}
}
package de.tudresden.inf.st.ros2rag.starter.data;
import java.util.ArrayList;
import java.util.List;
/**
* Data class for initial configuration.
*
* @author rschoene - Initial contribution
*/
public class DataConfiguration {
public List<DataJoint> joints = new ArrayList<>();
public String robotConfigTopic;
public String dataConfigTopic;
public int exitAfterSeconds = 0;
public String mqttHost = "localhost";
}
package de.tudresden.inf.st.ros2rag.starter.data;
/**
* Data class to describe a joint.
*
* @author rschoene - Initial contribution
*/
public class DataJoint {
public String topic;
public String name;
public boolean isEndEffector = false;
}
{
"joints": [
{"name": "Joint0", "topic": "panda_link_0"},
{"name": "Joint1", "topic": "panda_link_1"},
{"name": "Joint2", "topic": "panda_link_2"},
{"name": "Joint3", "topic": "panda_link_3"},
{"name": "Joint4", "topic": "panda_link_4"},
{"name": "Joint5", "topic": "panda_link_5"},
{"name": "Joint6", "topic": "panda_link_6"},
{"name": "EndEffector", "topic": "panda_link_7", "isEndEffector": true},
{"name": "LeftFinger", "topic": "panda_link_8"},
{"name": "RightFinger", "topic": "panda_link_9"}
],
"robotConfigTopic": "robotconfig",
"dataConfigTopic": "dataconfig"
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment