diff --git a/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/SimpleMain.java b/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/SimpleMain.java index 7c5412515d03e40503b22f1bc5f080eafac33aca..74406f87a166732efedafbc772d114185f24e227 100644 --- a/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/SimpleMain.java +++ b/ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/SimpleMain.java @@ -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); diff --git a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java b/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java index a7441813bec7621e12c1545afbda4d8fd0e0e109..2511df6e88b64f6c103745c907297852c8ab912e 100644 --- a/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java +++ b/ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java @@ -1,12 +1,25 @@ 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(); + } + + } + } diff --git a/ros2rag.starter/build.gradle b/ros2rag.starter/build.gradle index 3bf47eb898012149b48143ae32daf40945650d62..42aa7ecc0de7c5201e7847f41d607a79e8cbc1e6 100644 --- a/ros2rag.starter/build.gradle +++ b/ros2rag.starter/build.gradle @@ -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') diff --git a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag index 5317f5225ba755abc87a5f38978ed681ed078032..da9ce33855f7e8cb4a17bf27fa930b407e84b066 100644 --- a/ros2rag.starter/src/main/jastadd/Definitions.ros2rag +++ b/ros2rag.starter/src/main/jastadd/Definitions.ros2rag @@ -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(); :} diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java similarity index 58% rename from ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java rename to ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java index 81fd5bc7052ee2ad4d832b49bcf76734eed86949..f6939a38cc4d1f02a7a8a4c81575eec82a936feb 100644 --- a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java @@ -1,9 +1,14 @@ 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); + } } diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java new file mode 100644 index 0000000000000000000000000000000000000000..b2ec3639dc485971c58a91b0cd4916b37e79c0b2 --- /dev/null +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java @@ -0,0 +1,17 @@ +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"; +} diff --git a/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java new file mode 100644 index 0000000000000000000000000000000000000000..2431827d30a9742f0366283c39d1dc156b876da2 --- /dev/null +++ b/ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java @@ -0,0 +1,12 @@ +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; +} diff --git a/ros2rag.starter/src/main/resources/config.json b/ros2rag.starter/src/main/resources/config.json new file mode 100644 index 0000000000000000000000000000000000000000..af5e2b9835bd31e60418d97a8162f0c2fdb82215 --- /dev/null +++ b/ros2rag.starter/src/main/resources/config.json @@ -0,0 +1,16 @@ +{ + "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" +}