Martin Kunz 2 years ago
parent
commit
abecb10e31

+ 9 - 13
src/main/java/at/acdp/urweb/rtde/CommandType.java

@@ -1,17 +1,13 @@
 package at.acdp.urweb.rtde;
 
-public enum CommandType {
-    RTDE_REQUEST_PROTOCOL_VERSION(86),
-    RTDE_GET_URCONTROL_VERSION(118),
-    RTDE_TEXT_MESSAGE(77),
-    RTDE_DATA_PACKAGE(85),
-    RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS(79),
-    RTDE_CONTROL_PACKAGE_SETUP_INPUTS(118),
-    RTDE_CONTROL_PACKAGE_START(83),
-    RTDE_CONTROL_PACKAGE_PAUSE(80)
-    ;
+public class CommandType {
+    public final static int RTDE_REQUEST_PROTOCOL_VERSION = 86;
+    public final static int RTDE_GET_URCONTROL_VERSION = 118;
+    public final static int RTDE_TEXT_MESSAGE = 77;
+    public final static int RTDE_DATA_PACKAGE = 85;
+    public final static int RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79;
+    public final static int RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 118;
+    public final static int RTDE_CONTROL_PACKAGE_START = 83;
+    public final static int RTDE_CONTROL_PACKAGE_PAUSE = 80;
 
-    private final int val;
-    CommandType(int v) { val = v; }
-    public int getVal() { return val; }
 }

+ 63 - 172
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -1,19 +1,30 @@
 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 org.slf4j.LoggerFactory;
 
 import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
 import java.net.Socket;
+import java.net.SocketException;
+import java.net.UnknownHostException;
 import java.nio.ByteBuffer;
 import java.util.List;
+import java.util.concurrent.Semaphore;
 
-import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
-import static at.acdp.urweb.rtde.CommandType.RTDE_REQUEST_PROTOCOL_VERSION;
+import static at.acdp.urweb.rtde.CommandType.*;
 
 public class RTDEClient implements Runnable {
     private final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
+    private final Semaphore waitingRequestSem = new Semaphore(0);
+    private final Semaphore socketOpen = new Semaphore(0);
+
+    private volatile IRtdeData waitingRequest=null;
+
     public static int RTDE_PROTOCOL_VERSION = 2;
     private final String ip;
     private final int port;
@@ -27,201 +38,81 @@ public class RTDEClient implements Runnable {
         this.port = port;
     }
 
-    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");
-            }
-            dos = new DataOutputStream(rt.getOutputStream());
-            dis = new DataInputStream(rt.getInputStream());
+    public void start() throws IOException, InterruptedException {
+        Thread readThread = new Thread(this);
+        readThread.start();
+        socketOpen.acquire();
+        init();
+    }
 
-            init();
-            //Thread readThread = new Thread(this);
-            //readThread.start();
-            // send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
-        }
+    private void request(IRtdeData data) throws IOException, InterruptedException {
+        waitingRequest=data;
+        data.send(dos);
+        waitingRequestSem.acquire();
+        return;
     }
 
     // Internal method that actually reads the data
-    private void readSocket() throws IOException {
+    private void readSocket() throws IOException, InterruptedException {
         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++) {
-                rtm[i] = dis.readDouble();
+            int length = dis.readShort();
+            int type=dis.readByte();
+            if(type==waitingRequest.getType()) {
+                waitingRequest.read(dis);
+                waitingRequestSem.release();
+            } else switch(type) {
+                case RTDE_TEXT_MESSAGE:
+                    var rpv = new RtdeTextMessage();
+                    request(rpv);
+                    logger.info("RTDE_TEXT_MESSAGE", rpv.text);
+                    break;
+
+                default: {
+                    logger.warn(String.format("Unexpected package type: %d", type));
+                    byte[] buf=new byte[length];
+                    dis.readFully(buf);
+                }
             }
         }
-
-    }
-
-    private void receive(RtdeData cmd) throws IOException {
-        logger.info(cmd.getType().toString());
-        cmd.read(dis);
     }
 
     @Override
     public void run() {
         try {
-            init();
-        } catch (IOException e) {
-            e.printStackTrace();
-        }
-
-        while (_running) {
-            try {
-                readSocket();
-            } catch (IOException e) {
-                e.printStackTrace();
+            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");
             }
+            dos = new DataOutputStream(rt.getOutputStream());
+            dis = new DataInputStream(rt.getInputStream());
+            socketOpen.release();
+            while (_running) {
+                try {
+                    readSocket();
+                } catch (IOException | InterruptedException e) {
+                    e.printStackTrace();
+                }
+            }
+        } catch (IOException  e) {
+            logger.warn("run",e);
         }
     }
 
-    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();
-    }
-
-    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) 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(rpv, bytes.array());
     }
 
-    public void init() throws IOException {
+    public void init() throws IOException, InterruptedException {
         var rpv = new RtdeRequestProtocolVersion();
-        sendAndReceive(rpv);
+        request(rpv);
         var ruv = new RtdeRequestURVersion();
-        sendAndReceive(ruv);
-
-
-    }
-
-    private interface RtdeData<T> {
-        CommandType getType();
-
-        int getSize();
-
-        void setReplySize(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 {
-            setReplySize(dis.readShort());
-            int cmd = dis.readByte();
-            if(cmd==getType().getVal())
-                return;
-            if (cmd==77) {
-                int mLength=dis.readByte();
-                byte[] mText = new byte[mLength];
-                dis.readFully(mText);
-                String m=new String(mText);
-                logger.info(m);
-                int sLength=dis.readByte();
-                byte[] sText = new byte[sLength];
-                dis.readFully(sText);
-                String s=new String(sText);
-                logger.info(s);
-                int warning=dis.readByte();
-                String w=String.valueOf(warning);
-
-            } else throw new IOException(String.format("Expected %d, got %d", getType().getVal(), cmd));
-        }
-
-    }
-
-    private class RtdeRequestURVersion implements RtdeData {
-        private int replySize;
-        private int major, minor, bugfix, build;
-
-        @Override
-        public CommandType getType() {
-            return RTDE_GET_URCONTROL_VERSION;
-        }
-
-        @Override
-        public int getSize() {
-            return 3;
-        }
-
-        @Override
-        public void setReplySize(int s) {
-            replySize = s;
-        }
-
-        @Override
-        public RtdeRequestURVersion read(DataInputStream di) throws IOException {
-            readHeader(di);
-            major=di.readInt();
-            minor=di.readInt();
-            bugfix=di.readInt();
-            build=di.readInt();
-
-            return this;
-        }
-
-        @Override
-        public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
-            sendHeader(dos);
-            return this;
-        }
-    }
-
-    private class RtdeRequestProtocolVersion implements RtdeData {
-        public boolean success;
-        private int replySize;
-
-        @Override
-        public CommandType getType() {
-            return RTDE_REQUEST_PROTOCOL_VERSION;
-        }
-
-        @Override
-        public int getSize() {
-            return 5;
-        }
-
-        @Override
-        public void setReplySize(int size) {
-            this.replySize = 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;
-        }
+        request(ruv);
     }
 }

+ 51 - 0
src/main/java/at/acdp/urweb/rtde/packets/IRtdeData.java

@@ -0,0 +1,51 @@
+package at.acdp.urweb.rtde.packets;
+
+import at.acdp.urweb.rtde.RTDEClient;
+import org.slf4j.LoggerFactory;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+public interface IRtdeData<T> {
+    final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
+
+    int getType();
+
+    int getSize();
+
+    void setReplySize(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());
+    }
+
+    default void readHeader(DataInputStream dis) throws IOException {
+        setReplySize(dis.readShort());
+        int cmd = dis.readByte();
+        if(cmd==getType())
+            return;
+        if (cmd==77) {
+            int mLength=dis.readByte();
+            byte[] mText = new byte[mLength];
+            dis.readFully(mText);
+            String m=new String(mText);
+            logger.info(m);
+            int sLength=dis.readByte();
+            byte[] sText = new byte[sLength];
+            dis.readFully(sText);
+            String s=new String(sText);
+            logger.info(s);
+            int warning=dis.readByte();
+            String w=String.valueOf(warning);
+
+        } else throw new IOException(String.format("Expected %d, got %d", getType(), cmd));
+    }
+
+}

+ 48 - 0
src/main/java/at/acdp/urweb/rtde/packets/RtdeRequestProtocolVersion.java

@@ -0,0 +1,48 @@
+package at.acdp.urweb.rtde.packets;
+
+import at.acdp.urweb.rtde.CommandType;
+import at.acdp.urweb.rtde.RTDEClient;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_REQUEST_PROTOCOL_VERSION;
+
+public class RtdeRequestProtocolVersion implements IRtdeData {
+    public boolean success;
+    private int replySize;
+
+    public RtdeRequestProtocolVersion() {
+    }
+
+    @Override
+    public int getType() {
+        return RTDE_REQUEST_PROTOCOL_VERSION;
+    }
+
+    @Override
+    public int getSize() {
+        return 5;
+    }
+
+    @Override
+    public void setReplySize(int size) {
+        this.replySize = size;
+    }
+
+    @Override
+    public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
+        readHeader(d);
+        success = d.readBoolean();
+        return this;
+    }
+
+    @Override
+    public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
+        sendHeader(dos);
+        dos.writeShort(RTDEClient.RTDE_PROTOCOL_VERSION);
+        dos.flush();
+        return this;
+    }
+}

+ 50 - 0
src/main/java/at/acdp/urweb/rtde/packets/RtdeRequestURVersion.java

@@ -0,0 +1,50 @@
+package at.acdp.urweb.rtde.packets;
+
+import at.acdp.urweb.rtde.CommandType;
+import at.acdp.urweb.rtde.RTDEClient;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
+
+public class RtdeRequestURVersion implements IRtdeData {
+    private int replySize;
+    public int major;
+    public int minor;
+    public int bugfix;
+    public int build;
+
+    @Override
+    public int getType() {
+        return RTDE_GET_URCONTROL_VERSION;
+    }
+
+    @Override
+    public int getSize() {
+        return 3;
+    }
+
+    @Override
+    public void setReplySize(int s) {
+        replySize = s;
+    }
+
+    @Override
+    public RtdeRequestURVersion read(DataInputStream di) throws IOException {
+        readHeader(di);
+        major=di.readInt();
+        minor=di.readInt();
+        bugfix=di.readInt();
+        build=di.readInt();
+
+        return this;
+    }
+
+    @Override
+    public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
+        sendHeader(dos);
+        return this;
+    }
+}

+ 51 - 0
src/main/java/at/acdp/urweb/rtde/packets/RtdeTextMessage.java

@@ -0,0 +1,51 @@
+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_GET_URCONTROL_VERSION;
+import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
+
+public class RtdeTextMessage implements IRtdeData {
+    private int replySize;
+    public String text;
+    public String source;
+    public int warningLevel;
+
+    @Override
+    public int getType() {
+        return RTDE_TEXT_MESSAGE;
+    }
+
+    @Override
+    public int getSize() {
+        return 3;
+    }
+
+    @Override
+    public void setReplySize(int s) {
+        replySize = s;
+    }
+
+    @Override
+    public RtdeTextMessage read(DataInputStream di) throws IOException {
+        readHeader(di);
+        int mLength=di.readByte();
+        byte[] msgb=new byte[mLength];
+        di.readFully(msgb);
+        text=new String(msgb);
+        int sLength=di.readByte();
+        byte[] sourceb=new byte[sLength];
+        di.readFully(sourceb);
+        source=new String(sourceb);
+        warningLevel=di.readByte();
+        return this;
+    }
+
+    @Override
+    public RtdeTextMessage send(DataOutputStream dos) throws IOException {
+        sendHeader(dos);
+        return this;
+    }
+}