|
@@ -2,32 +2,33 @@ package at.acdp.urweb.rtde;
|
|
|
|
|
|
import org.slf4j.LoggerFactory;
|
|
|
|
|
|
-import java.io.*;
|
|
|
+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.List;
|
|
|
|
|
|
+import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
|
|
|
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);
|
|
|
-
|
|
|
+ public static int RTDE_PROTOCOL_VERSION = 2;
|
|
|
private final String ip;
|
|
|
private final int port;
|
|
|
- private volatile boolean _running=true;
|
|
|
+ private final boolean _running = true;
|
|
|
private DataOutputStream dos;
|
|
|
private DataInputStream dis;
|
|
|
- public static int RTDE_PROTOCOL_VERSION=2;
|
|
|
|
|
|
|
|
|
public RTDEClient(String ip, int port) {
|
|
|
- this.ip=ip;
|
|
|
- this.port=port;
|
|
|
+ this.ip = ip;
|
|
|
+ this.port = port;
|
|
|
}
|
|
|
|
|
|
public void start() throws IOException {
|
|
|
- try (Socket rt = new Socket(ip, port);) {
|
|
|
+ try (Socket rt = new Socket(ip, port)) {
|
|
|
rt.setSoTimeout(0);
|
|
|
rt.setReuseAddress(true);
|
|
|
rt.setTcpNoDelay(true);
|
|
@@ -46,55 +47,19 @@ public class RTDEClient implements Runnable {
|
|
|
|
|
|
// Internal method that actually reads the data
|
|
|
private void readSocket() throws IOException {
|
|
|
- while(true) {
|
|
|
+ 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++){
|
|
|
+ for (int i = 0; i < data_available; i++) {
|
|
|
rtm[i] = dis.readDouble();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
- private interface RtdeData<T> {
|
|
|
- public CommandType getType();
|
|
|
- public T read(DataInputStream di) throws IOException;
|
|
|
- public T send(DataOutputStream dos) throws IOException;
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
- private class RtdeRequestProtocolVersion implements RtdeData{
|
|
|
- public int major, minor, bugfix, build;
|
|
|
- int size=5;
|
|
|
-
|
|
|
- @Override
|
|
|
- public CommandType getType() {
|
|
|
- return RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
|
|
|
- major = d.readInt();
|
|
|
- minor = d.readInt();
|
|
|
- bugfix = d.readInt();
|
|
|
- build = d.readInt();
|
|
|
- return this;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
|
|
|
- dos.writeByte(5);
|
|
|
- dos.writeShort(getType().getVal());
|
|
|
- dos.writeShort(RTDE_PROTOCOL_VERSION);
|
|
|
- dos.flush();
|
|
|
- return this;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
private void receive(RtdeData cmd) throws IOException {
|
|
|
logger.info(cmd.getType().toString());
|
|
|
cmd.read(dis);
|
|
@@ -108,7 +73,7 @@ public class RTDEClient implements Runnable {
|
|
|
e.printStackTrace();
|
|
|
}
|
|
|
|
|
|
- while(_running) {
|
|
|
+ while (_running) {
|
|
|
try {
|
|
|
readSocket();
|
|
|
} catch (IOException e) {
|
|
@@ -128,20 +93,110 @@ public class RTDEClient implements Runnable {
|
|
|
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);
|
|
|
+ byte[] p = payload.getBytes();
|
|
|
+ ByteBuffer bytes = ByteBuffer.allocate(8 + p.length);
|
|
|
bytes.putDouble(frequency);
|
|
|
bytes.put(p);
|
|
|
|
|
|
// sendAndReceive(rpv, bytes.array());
|
|
|
}
|
|
|
|
|
|
-
|
|
|
public void negotiate_protocol_version() throws IOException {
|
|
|
var rpv = new RtdeRequestProtocolVersion();
|
|
|
sendAndReceive(rpv);
|
|
|
}
|
|
|
+
|
|
|
+ private interface RtdeData<T> {
|
|
|
+ CommandType getType();
|
|
|
+
|
|
|
+ int getSize();
|
|
|
+
|
|
|
+ void setSize(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 {
|
|
|
+ setSize(dis.readShort());
|
|
|
+ int cmd = dis.readByte();
|
|
|
+ if (cmd != getType().getVal())
|
|
|
+ throw new IOException(String.format("Expected %d, got %d", getType().getVal(), cmd));
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ private class RtdeRequestURVersion implements RtdeData {
|
|
|
+ private int size;
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public CommandType getType() {
|
|
|
+ return RTDE_GET_URCONTROL_VERSION;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public int getSize() {
|
|
|
+ return 5;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public void setSize(int s) {
|
|
|
+ size = s;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public RtdeRequestURVersion read(DataInputStream di) throws IOException {
|
|
|
+ readHeader(di);
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
|
|
|
+ sendHeader(dos);
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private class RtdeRequestProtocolVersion implements RtdeData {
|
|
|
+ public boolean success;
|
|
|
+ private int size;
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public CommandType getType() {
|
|
|
+ return RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public int getSize() {
|
|
|
+ return size;
|
|
|
+ }
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public void setSize(int size) {
|
|
|
+ this.size = 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|