uraxisdaemon/src/main/java/at/acdp/urweb/rtde/RTDEClient.java

package at.acdp.urweb.rtde;

import net.openhft.chronicle.bytes.Bytes;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.List;

public class RTDEClient implements Runnable {

    private final String ip;
    private final int port;
    private volatile boolean _running=true;
    private DataOutputStream dos;
    private DataInputStream dis;


    public RTDEClient(String ip, int port) {
        this.ip=ip;
        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);){
            rt.setSoTimeout(0);
            if (rt.isConnected()){
                System.out.println("Connected to UR Realtime Client");
            }
            dis = new DataInputStream(rt.getInputStream());
            dos = new DataOutputStream(rt.getOutputStream());


            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
    public void run() {
        while(_running) {
            try {
                readSocket();
            } catch (IOException e) {
                e.printStackTrace();
            }
        }
    }

    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());
    }

    private void sendAndReceive(CommandType cmd, byte[] payload) {
        sendall(cmd, payload);
    }


    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());
    }

    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());
            dos.flush();
        } catch (IOException e) {
            e.printStackTrace();
        }
    }
}