Martin Kunz 2 years ago
parent
commit
ed353df1e6
1 changed files with 106 additions and 51 deletions
  1. 106 51
      src/main/java/at/acdp/urweb/rtde/RTDEClient.java

+ 106 - 51
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -2,32 +2,33 @@ package at.acdp.urweb.rtde;
 
 import org.slf4j.LoggerFactory;
 
-import java.io.*;
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
 import java.net.Socket;
 import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
 import java.util.List;
 
+import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
 import static at.acdp.urweb.rtde.CommandType.RTDE_REQUEST_PROTOCOL_VERSION;
 
 public class RTDEClient implements Runnable {
     private final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
-
+    public static int RTDE_PROTOCOL_VERSION = 2;
     private final String ip;
     private final int port;
-    private volatile boolean _running=true;
+    private final boolean _running = true;
     private DataOutputStream dos;
     private DataInputStream dis;
-    public static int RTDE_PROTOCOL_VERSION=2;
 
 
     public RTDEClient(String ip, int port) {
-        this.ip=ip;
-        this.port=port;
+        this.ip = ip;
+        this.port = port;
     }
 
     public void start() throws IOException {
-        try (Socket rt = new Socket(ip, port);) {
+        try (Socket rt = new Socket(ip, port)) {
             rt.setSoTimeout(0);
             rt.setReuseAddress(true);
             rt.setTcpNoDelay(true);
@@ -46,55 +47,19 @@ public class RTDEClient implements Runnable {
 
     // Internal method that actually reads the data
     private void readSocket() throws IOException {
-        while(true) {
+        while (true) {
             int length = dis.readInt();
             double[] rtm = new double[length];
             rtm[0] = length;
             // Calculate how much data is available from the length
             int data_available = (length - 4) / 8;
-            for(int i=0; i<data_available; i++){
+            for (int i = 0; i < data_available; i++) {
                 rtm[i] = dis.readDouble();
             }
         }
 
     }
 
-    private interface RtdeData<T> {
-        public CommandType getType();
-        public T read(DataInputStream di) throws IOException;
-        public T send(DataOutputStream dos) throws IOException;
-
-
-    }
-    private class RtdeRequestProtocolVersion implements RtdeData{
-        public  int major, minor, bugfix, build;
-        int size=5;
-
-        @Override
-        public CommandType getType() {
-            return RTDE_REQUEST_PROTOCOL_VERSION;
-        }
-
-        @Override
-        public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
-             major = d.readInt();
-             minor = d.readInt();
-             bugfix = d.readInt();
-             build = d.readInt();
-             return this;
-        }
-
-        @Override
-        public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
-            dos.writeByte(5);
-            dos.writeShort(getType().getVal());
-            dos.writeShort(RTDE_PROTOCOL_VERSION);
-            dos.flush();
-            return this;
-        }
-    }
-
-
     private void receive(RtdeData cmd) throws IOException {
         logger.info(cmd.getType().toString());
         cmd.read(dis);
@@ -108,7 +73,7 @@ public class RTDEClient implements Runnable {
             e.printStackTrace();
         }
 
-        while(_running) {
+        while (_running) {
             try {
                 readSocket();
             } catch (IOException e) {
@@ -128,20 +93,110 @@ public class RTDEClient implements Runnable {
         cmd.read(dis);
     }
 
-
     public void send_output_setup(List<String> variables, List<String> types, int frequency) throws IOException {
         var payload = String.join(",", variables);
-        byte[] p= payload.getBytes();
-        ByteBuffer bytes = ByteBuffer.allocate(8+p.length);
+        byte[] p = payload.getBytes();
+        ByteBuffer bytes = ByteBuffer.allocate(8 + p.length);
         bytes.putDouble(frequency);
         bytes.put(p);
 
         // sendAndReceive(rpv, bytes.array());
     }
 
-
     public void negotiate_protocol_version() throws IOException {
         var rpv = new RtdeRequestProtocolVersion();
         sendAndReceive(rpv);
     }
+
+    private interface RtdeData<T> {
+        CommandType getType();
+
+        int getSize();
+
+        void setSize(int i);
+
+
+        T read(DataInputStream di) throws IOException;
+
+        T send(DataOutputStream dos) throws IOException;
+
+        default void sendHeader(DataOutputStream dos) throws IOException {
+            dos.writeShort(getSize());
+            dos.writeByte(getType().getVal());
+        }
+
+        default void readHeader(DataInputStream dis) throws IOException {
+            setSize(dis.readShort());
+            int cmd = dis.readByte();
+            if (cmd != getType().getVal())
+                throw new IOException(String.format("Expected %d, got %d", getType().getVal(), cmd));
+        }
+
+    }
+
+    private class RtdeRequestURVersion implements RtdeData {
+        private int size;
+
+        @Override
+        public CommandType getType() {
+            return RTDE_GET_URCONTROL_VERSION;
+        }
+
+        @Override
+        public int getSize() {
+            return 5;
+        }
+
+        @Override
+        public void setSize(int s) {
+            size = s;
+        }
+
+        @Override
+        public RtdeRequestURVersion read(DataInputStream di) throws IOException {
+            readHeader(di);
+            return this;
+        }
+
+        @Override
+        public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
+            sendHeader(dos);
+            return this;
+        }
+    }
+
+    private class RtdeRequestProtocolVersion implements RtdeData {
+        public boolean success;
+        private int size;
+
+        @Override
+        public CommandType getType() {
+            return RTDE_REQUEST_PROTOCOL_VERSION;
+        }
+
+        @Override
+        public int getSize() {
+            return size;
+        }
+
+        @Override
+        public void setSize(int size) {
+            this.size = size;
+        }
+
+        @Override
+        public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
+            readHeader(dis);
+            success = dis.readBoolean();
+            return this;
+        }
+
+        @Override
+        public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
+            sendHeader(dos);
+            dos.writeShort(RTDE_PROTOCOL_VERSION);
+            dos.flush();
+            return this;
+        }
+    }
 }