Martin Kunz 2 years ago
parent
commit
4650c7ffa0
1 changed files with 81 additions and 7 deletions
  1. 81 7
      src/main/java/at/acdp/urweb/rtde/RTDEClient.java

+ 81 - 7
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -1,6 +1,6 @@
 package at.acdp.urweb.rtde;
 
-import net.openhft.chronicle.bytes.Bytes;
+import org.slf4j.LoggerFactory;
 
 import java.io.DataInputStream;
 import java.io.DataOutputStream;
@@ -8,16 +8,19 @@ import java.io.IOException;
 import java.net.Socket;
 import java.nio.ByteBuffer;
 import java.nio.ByteOrder;
-import java.util.ArrayList;
 import java.util.List;
 
+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);
 
     private final String ip;
     private final int port;
     private volatile boolean _running=true;
     private DataOutputStream dos;
     private DataInputStream dis;
+    public static int RTDE_PROTOCOL_VERSION=2;
 
 
     public RTDEClient(String ip, int port) {
@@ -34,8 +37,9 @@ public class RTDEClient implements Runnable {
             dis = new DataInputStream(rt.getInputStream());
             dos = new DataOutputStream(rt.getOutputStream());
             Thread readThread = new Thread(this);
-            readThread.start();
-            send_output_setup(List.of(OutParams.speed_scaling), List.of(), 125);
+            //readThread.start();
+            negotiate_protocol_version();
+            send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
         }
     }
 
@@ -54,6 +58,65 @@ public class RTDEClient implements Runnable {
 
     }
 
+    private interface RtdeData<T> {
+        public CommandType getType();
+        public T read(DataInputStream di) throws IOException;
+
+    }
+    private class RtdeRequestProtocolVersion implements RtdeData{
+
+        public  int major;
+        public  int minor;
+        public  int bugfix;
+        public  int build;
+
+        @Override
+        public CommandType getType() {
+            return RTDE_REQUEST_PROTOCOL_VERSION;
+        }
+
+        public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
+             major = d.readInt();
+             minor = d.readInt();
+             bugfix = d.readInt();
+             build = d.readInt();
+             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;
+    }
+
     @Override
     public void run() {
         while(_running) {
@@ -68,11 +131,13 @@ public class RTDEClient implements Runnable {
     public void send_input_setup(List<String> variables, List<String> types) {
         var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
         var payload = String.join(",", variables);
-        sendAndReceive(cmd, payload.getBytes());
+        var rpv = new RtdeRequestProtocolVersion();
+        sendAndReceive(rpv, payload.getBytes());
     }
 
-    private void sendAndReceive(CommandType cmd, byte[] payload) {
+    private void sendAndReceive(RtdeData cmd, byte[] payload) {
         sendall(cmd, payload);
+        receive(cmd);
     }
 
 
@@ -86,7 +151,16 @@ public class RTDEClient implements Runnable {
         sendAndReceive(cmd, bytes.array());
     }
 
-    public void  sendall(CommandType cmd, byte[] payload) {
+
+    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());
+    }
+
+
+    public void  sendall(RtdeData cmd, byte[] payload) {
         try {
             int size = 3 + payload.length;
             ByteBuffer bytes = ByteBuffer.allocate(size);