123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108 |
- 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.nio.ByteOrder;
- 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() 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());
- 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 {
- 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) {
- var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
- 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 sendall(CommandType 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();
- }
- }
- }
|