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

+ 39 - 73
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -2,9 +2,7 @@ package at.acdp.urweb.rtde;
 
 import org.slf4j.LoggerFactory;
 
-import java.io.DataInputStream;
-import java.io.DataOutputStream;
-import java.io.IOException;
+import java.io.*;
 import java.net.Socket;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
@@ -31,15 +29,18 @@ public class RTDEClient implements Runnable {
     public void start() throws IOException {
         try (Socket rt = new Socket(ip, port);) {
             rt.setSoTimeout(0);
+            rt.setReuseAddress(true);
+            rt.setTcpNoDelay(true);
             if (rt.isConnected()) {
                 System.out.println("Connected to UR Realtime Client");
             }
-            dis = new DataInputStream(rt.getInputStream());
             dos = new DataOutputStream(rt.getOutputStream());
-            Thread readThread = new Thread(this);
-            //readThread.start();
+            dis = new DataInputStream(rt.getInputStream());
+
             negotiate_protocol_version();
-            send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
+            //Thread readThread = new Thread(this);
+            //readThread.start();
+            // send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
         }
     }
 
@@ -61,20 +62,20 @@ public class RTDEClient implements Runnable {
     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;
-        public  int minor;
-        public  int bugfix;
-        public  int build;
+        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();
@@ -82,43 +83,31 @@ public class RTDEClient implements Runnable {
              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 {
-        switch (cmd) {
-            case RTDE_REQUEST_PROTOCOL_VERSION:
-                logger.info("RTDE_REQUEST_PROTOCOL_VERSION");
-                var pv=new ProtocolVersion();
-                return pv.read(dis);
-                break;
-            case RTDE_GET_URCONTROL_VERSION:
-                logger.info("RTDE_GET_URCONTROL_VERSION");
-                break;
-            case RTDE_TEXT_MESSAGE:
-                logger.info("RTDE_TEXT_MESSAGE");
-                break;
-            case RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
-                logger.info("RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS");
-                break;
-            case RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
-                logger.info("RTDE_CONTROL_PACKAGE_SETUP_INPUTS");
-                break;
-            case RTDE_CONTROL_PACKAGE_START:
-                logger.info("RTDE_CONTROL_PACKAGE_START");
-                break;
-            case RTDE_CONTROL_PACKAGE_PAUSE:
-                logger.info("RTDE_CONTROL_PACKAGE_PAUSE");
-                break;
-            case RTDE_DATA_PACKAGE:
-                logger.info("RTDE_DATA_PACKAGE");
-                break;
-        }
-        return null;
+        logger.info(cmd.getType().toString());
+        cmd.read(dis);
     }
 
     @Override
     public void run() {
+        try {
+            negotiate_protocol_version();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+
         while(_running) {
             try {
                 readSocket();
@@ -128,54 +117,31 @@ public class RTDEClient implements Runnable {
         }
     }
 
-    public void send_input_setup(List<String> variables, List<String> types) {
+    public void send_input_setup(List<String> variables, List<String> types) throws IOException {
         var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
         var payload = String.join(",", variables);
         var rpv = new RtdeRequestProtocolVersion();
-        sendAndReceive(rpv, payload.getBytes());
     }
 
-    private void sendAndReceive(RtdeData cmd, byte[] payload) {
-        sendall(cmd, payload);
-        receive(cmd);
+    private void sendAndReceive(RtdeData cmd) throws IOException {
+        cmd.send(dos);
+        cmd.read(dis);
     }
 
 
-    public void send_output_setup(List<String> variables, List<String> types, int frequency) {
-        var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
+    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);
         bytes.putDouble(frequency);
         bytes.put(p);
-        sendAndReceive(cmd, bytes.array());
-    }
-
 
-    public void negotiate_protocol_version() {
-        var cmd = RTDE_REQUEST_PROTOCOL_VERSION;
-        ByteBuffer bytes = ByteBuffer.allocate(2);
-        bytes.putShort((short) RTDE_PROTOCOL_VERSION);
-        sendAndReceive(cmd, bytes.array());
+        // sendAndReceive(rpv, bytes.array());
     }
 
 
-    public void  sendall(RtdeData cmd, byte[] payload) {
-        try {
-            int size = 3 + payload.length;
-            ByteBuffer bytes = ByteBuffer.allocate(size);
-            bytes.order(ByteOrder.BIG_ENDIAN);
-            bytes.order( ByteOrder.BIG_ENDIAN);
-            bytes.putShort((short) size);
-            bytes.put((byte) cmd.getVal());
-            bytes.put(payload);
-            bytes.rewind();
-            byte[] arr = new byte[bytes.remaining()];
-            bytes.get(arr);
-            dos.write(arr);
-            dos.flush();
-        } catch (IOException e) {
-            e.printStackTrace();
-        }
+    public void negotiate_protocol_version() throws IOException {
+        var rpv = new RtdeRequestProtocolVersion();
+        sendAndReceive(rpv);
     }
 }