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

- Starter: Send initial DataConfig
- Starter: Always send complete RobotConfig

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);

ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java

 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,34 +30,48 @@ 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(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")) {
+        finish.countDown();
       }
     });
-    receiver.newConnection(jointTopic, bytes -> {
+
+    Runtime.getRuntime().addShutdownHook(new Thread(receiver::close));
+    if (config.exitAfterSeconds > 0) {
+      finish.await(config.exitAfterSeconds, TimeUnit.SECONDS);
+    } else {
+      finish.await();
+    }
+    receiver.close();
+  }
+
+  private void printPandaLinkState(byte[] bytes) {
     try {
       logger.debug("Got a joint message, parsing ...");
       PandaLinkState pls = PandaLinkState.parseFrom(bytes);
@@ -71,29 +98,35 @@ public class Main {
     } catch (InvalidProtocolBufferException e) {
       e.printStackTrace();
     }
-    });
-    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")) {
+  }
+
+  private void printRobotConfig(byte[] bytes) {
     try {
-          finishLock.lock();
-          finishCondition.signal();
-        } finally {
-          finishLock.unlock();
+      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 {
-      finishLock.lock();
-      finishCondition.await();
-    } finally {
-      finishLock.unlock();
+      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();
     }
-    receiver.close();
-    Runtime.getRuntime().addShutdownHook(new Thread(receiver::close));
+
   }
+
 }

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')

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();
 :}

ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java → ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/StarterMain.java

 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,36 +18,14 @@ 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},
@@ -52,8 +35,12 @@ public class Main {
 
     // --- 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);
+  }
 }

ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataConfiguration.java

+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";
+}

ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/data/DataJoint.java

+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;
+}

ros2rag.starter/src/main/resources/config.json

+{
+  "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"
+}