rtde: data packets parsing

src/main/java/at/acdp/urweb/Main.java

@@ -5,6 +5,7 @@ import at.acdp.urweb.fhpp.FHMasterSim;
 import at.acdp.urweb.fhpp.FHPP;
 import at.acdp.urweb.fhpp.IFHMaster;
 import at.acdp.urweb.rtde.RTDEClient;
+import at.acdp.urweb.rtde.packets.RtdeControlStart;
 import at.acdp.urweb.rtde.packets.RtdeSetupOutputs;
 import at.acdp.urweb.web.FestoXRServer;
 import com.nmote.nanohttp.NanoServer;
@@ -28,9 +29,17 @@ public class Main {
             r.start();
             var so=new RtdeSetupOutputs()
                     .addVariable("timestamp")
-                    .addVariable("robot_mode");
+                    .addVariable("robot_mode")
+                    .addVariable("safety_mode")
+                    .addVariable("safety_status")
+                    .addVariable("target_speed_fraction")
+                    .addVariable("speed_scaling");
             r.request(so);
 
+            var start=new RtdeControlStart();
+            r.request(start);
+
+
             FestoXRServer fxrs = new FestoXRServer();
             HTTPServerEndpoint xrs = XR.server(fxrs, FestoXRServer.class);
             server.add(xrs);

src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -1,15 +1,13 @@
 package at.acdp.urweb.rtde;
 
-import at.acdp.urweb.rtde.packets.IRtdeData;
-import at.acdp.urweb.rtde.packets.RtdeRequestProtocolVersion;
-import at.acdp.urweb.rtde.packets.RtdeRequestURVersion;
-import at.acdp.urweb.rtde.packets.RtdeTextMessage;
+import at.acdp.urweb.rtde.packets.*;
 import org.slf4j.LoggerFactory;
 
 import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
 import java.net.Socket;
+import java.util.HashMap;
 import java.util.concurrent.Semaphore;
 
 import static at.acdp.urweb.rtde.CommandType.*;
@@ -28,6 +26,8 @@ public class RTDEClient implements Runnable {
     private DataOutputStream dos;
     private DataInputStream dis;
     private Thread readThread;
+    private RtdeSetupOutputs outputs;
+    private HashMap<String, String> lastData;
 
 
     public RTDEClient(String ip, int port) {
@@ -42,6 +42,10 @@ public class RTDEClient implements Runnable {
         init();
     }
 
+    public HashMap<String, String> getLastData() {
+        return lastData;
+    }
+
     public void request(IRtdeData data) throws IOException, InterruptedException {
         if(readThread.getId()==Thread.currentThread().getId()) {
             logger.error("request() must not be called from readThread.");
@@ -49,6 +53,7 @@ public class RTDEClient implements Runnable {
         waitingRequest=data;
         data.send(dos);
         waitingRequestSem.acquire();
+        waitingRequest=null;
         return;
     }
 
@@ -56,15 +61,27 @@ public class RTDEClient implements Runnable {
     private void readSocket() throws IOException, InterruptedException {
         int length = dis.readShort();
         int type=dis.readByte();
-        if(type==waitingRequest.getType()) {
+        var wreq=waitingRequest;
+        if(wreq != null && type==wreq.getType()) {
             waitingRequest.read(dis, length-3);
+            if(type== RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS) {
+                outputs= (RtdeSetupOutputs) waitingRequest;
+            }
             waitingRequestSem.release();
         } else switch(type) {
             case RTDE_TEXT_MESSAGE:
                 var rpv = new RtdeTextMessage();
-                request(rpv);
+                rpv.read(dis, length-3);
                 logger.info("RTDE_TEXT_MESSAGE", rpv.text);
                 break;
+            case RTDE_DATA_PACKAGE:
+                var p = new RtdeDataPackage(outputs);
+                p.read(dis, length-3);
+                lastData=p.getPdata();
+                lastData.entrySet().forEach(entry->{
+                    System.out.println(entry.getKey() + " " + entry.getValue());
+                });
+                break;
 
             default: {
                 logger.warn(String.format("Unexpected package type: %d", type));

src/main/java/at/acdp/urweb/rtde/packets/RtdeControlPause.java

@@ -0,0 +1,29 @@
+package at.acdp.urweb.rtde.packets;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_CONTROL_PACKAGE_PAUSE;
+import static at.acdp.urweb.rtde.CommandType.RTDE_CONTROL_PACKAGE_START;
+
+public class RtdeControlPause implements IRtdeData {
+    public boolean accepted;
+
+    @Override
+    public int getType() {
+        return RTDE_CONTROL_PACKAGE_PAUSE;
+    }
+
+    @Override
+    public RtdeControlPause read(DataInputStream di, int length) throws IOException {
+        accepted= di.readBoolean();
+        return this;
+    }
+
+    @Override
+    public RtdeControlPause send(DataOutputStream dos) throws IOException {
+        sendHeader(dos,0);
+        return this;
+    }
+}

src/main/java/at/acdp/urweb/rtde/packets/RtdeControlStart.java

@@ -0,0 +1,29 @@
+package at.acdp.urweb.rtde.packets;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_CONTROL_PACKAGE_START;
+import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
+
+public class RtdeControlStart implements IRtdeData {
+    public boolean accepted;
+
+    @Override
+    public int getType() {
+        return RTDE_CONTROL_PACKAGE_START;
+    }
+
+    @Override
+    public RtdeControlStart read(DataInputStream di, int length) throws IOException {
+        accepted= di.readBoolean();
+        return this;
+    }
+
+    @Override
+    public RtdeControlStart send(DataOutputStream dos) throws IOException {
+        sendHeader(dos,0);
+        return this;
+    }
+}

src/main/java/at/acdp/urweb/rtde/packets/RtdeDataPackage.java

@@ -0,0 +1,78 @@
+package at.acdp.urweb.rtde.packets;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.HashMap;
+import java.util.concurrent.ConcurrentHashMap;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
+
+public class RtdeDataPackage implements IRtdeData {
+    private final RtdeSetupOutputs outputs;
+    private HashMap<String, String> pdata=new HashMap<>();
+
+
+    public RtdeDataPackage(RtdeSetupOutputs outputs) {
+        this.outputs=outputs;
+    }
+
+    @Override
+    public int getType() {
+        return RTDE_TEXT_MESSAGE;
+    }
+
+    public HashMap<String, String> getPdata() {
+        return pdata;
+    }
+
+    @Override
+    public RtdeDataPackage read(DataInputStream di, int length) throws IOException {
+        int recipeID=di.readByte();
+        for(int i=0; i<outputs.variables.size(); i++) {
+            String output=outputs.variables.get(i);
+            String type=outputs.types.get(i);
+            switch (type) {
+                case "BOOL":
+                    pdata.put(output, String.valueOf(di.readBoolean()));
+                    break;
+                case "UINT8":
+                    pdata.put(output, String.valueOf(di.readByte()));
+                    break;
+                case "UINT32":
+                case "INT32":
+                    pdata.put(output, String.valueOf(di.readInt()));
+                    break;
+                case "UINT64":
+                    pdata.put(output, String.valueOf(di.readLong()));
+                    break;
+                case "DOUBLE":
+                    pdata.put(output, String.valueOf(di.readDouble()));
+                    break;
+                case "VECTOR3D":
+                    pdata.put(output, String.format("%f %f %f",di.readDouble(), di.readDouble(), di.readDouble()));
+                    break;
+                case "VECTOR6D":
+                    pdata.put(output, String.format("%f %f %f %f %f %f",di.readDouble(), di.readDouble(), di.readDouble(), di.readDouble(), di.readDouble(), di.readDouble()));
+                    break;
+                case "VECTOR6INT32":
+                    pdata.put(output, String.format("%d %d %d",di.readInt(), di.readInt(), di.readInt()));
+                    break;
+                case "VECTOR6UINT32":
+                    pdata.put(output, String.format("%d %d %d %d %d %d",di.readInt(), di.readInt(), di.readInt(), di.readInt(), di.readInt(), di.readInt()));
+                    break;
+
+                default:
+                    throw new IllegalStateException("Unexpected value: " + type);
+            }
+        }
+
+        return this;
+    }
+
+    @Override
+    public RtdeDataPackage send(DataOutputStream dos) throws IOException {
+        sendHeader(dos,0);
+        return this;
+    }
+}

src/main/java/at/acdp/urweb/rtde/packets/RtdeSetupOutputs.java

@@ -4,13 +4,15 @@ import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 
 import static at.acdp.urweb.rtde.CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
 
 public class RtdeSetupOutputs implements IRtdeData {
     public int recipe;
-    private List<String> variables=new ArrayList<>();
+    public List<String> variables=new ArrayList<>();
+    public List<String> types=new ArrayList<>();
     public String typesText;
 
     public RtdeSetupOutputs() {
@@ -32,6 +34,7 @@ public class RtdeSetupOutputs implements IRtdeData {
         byte[] text=new byte[length-1];
         d.readFully(text);
         typesText=new String(text);
+        types.addAll(Arrays.asList(typesText.split(",")));
         return this;
     }