Martin Kunz 2 years ago
parent
commit
88b9142871
1 changed files with 35 additions and 28 deletions
  1. 35 28
      src/main/java/at/acdp/urweb/rtde/RTDEClient.java

+ 35 - 28
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -7,6 +7,7 @@ import java.io.DataOutputStream;
 import java.io.IOException;
 import java.net.Socket;
 import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -24,34 +25,33 @@ public class RTDEClient implements Runnable {
         this.port=port;
     }
 
-    public void start() {
-        Thread readThread = new Thread(this);
-        readThread.start();
-        send_output_setup(List.of(OutParams.speed_scaling),List.of(),125);
-    }
-
-    // Internal method that actually reads the data
-    private void readSocket() throws IOException {
-        try(Socket rt = new Socket(ip, port);){
+    public void start() throws IOException {
+        try (Socket rt = new Socket(ip, port);) {
             rt.setSoTimeout(0);
-            if (rt.isConnected()){
+            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();
+            send_output_setup(List.of(OutParams.speed_scaling), List.of(), 125);
+        }
+    }
 
-
-            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();
-                }
+    // Internal method that actually reads the data
+    private void readSocket() throws IOException {
+        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();
             }
         }
+
     }
 
     @Override
@@ -77,21 +77,28 @@ public class RTDEClient implements Runnable {
 
 
     public void send_output_setup(List<String> variables, List<String> types, int frequency) {
-        Bytes<ByteBuffer> bytes = Bytes.elasticHeapByteBuffer(64);
-        bytes.writeDouble(frequency);
         var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
         var payload = String.join(",", variables);
-        sendAndReceive(cmd, payload.getBytes());
+        byte[] p= payload.getBytes();
+        ByteBuffer bytes = ByteBuffer.allocate(8+p.length);
+        bytes.putDouble(frequency);
+        bytes.put(p);
+        sendAndReceive(cmd, bytes.array());
     }
 
     public void  sendall(CommandType cmd, byte[] payload) {
         try {
-            Bytes<ByteBuffer> bytes = Bytes.elasticHeapByteBuffer(64);
             int size = 3 + payload.length;
-            bytes.writeUnsignedShort(size);
-            bytes.writeUnsignedByte(cmd.getVal());
-            bytes.write(payload);
-            dos.write(bytes.toByteArray());
+            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();