|
@@ -1,19 +1,30 @@
|
|
|
package at.acdp.urweb.rtde;
|
|
|
|
|
|
+import at.acdp.urweb.rtde.packets.IRtdeData;
|
|
|
+import at.acdp.urweb.rtde.packets.RtdeRequestProtocolVersion;
|
|
|
+import at.acdp.urweb.rtde.packets.RtdeRequestURVersion;
|
|
|
+import at.acdp.urweb.rtde.packets.RtdeTextMessage;
|
|
|
import org.slf4j.LoggerFactory;
|
|
|
|
|
|
import java.io.DataInputStream;
|
|
|
import java.io.DataOutputStream;
|
|
|
import java.io.IOException;
|
|
|
import java.net.Socket;
|
|
|
+import java.net.SocketException;
|
|
|
+import java.net.UnknownHostException;
|
|
|
import java.nio.ByteBuffer;
|
|
|
import java.util.List;
|
|
|
+import java.util.concurrent.Semaphore;
|
|
|
|
|
|
-import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
|
|
|
-import static at.acdp.urweb.rtde.CommandType.RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
+import static at.acdp.urweb.rtde.CommandType.*;
|
|
|
|
|
|
public class RTDEClient implements Runnable {
|
|
|
private final static org.slf4j.Logger logger = LoggerFactory.getLogger(RTDEClient.class);
|
|
|
+ private final Semaphore waitingRequestSem = new Semaphore(0);
|
|
|
+ private final Semaphore socketOpen = new Semaphore(0);
|
|
|
+
|
|
|
+ private volatile IRtdeData waitingRequest=null;
|
|
|
+
|
|
|
public static int RTDE_PROTOCOL_VERSION = 2;
|
|
|
private final String ip;
|
|
|
private final int port;
|
|
@@ -27,201 +38,81 @@ public class RTDEClient implements Runnable {
|
|
|
this.port = port;
|
|
|
}
|
|
|
|
|
|
- public void start() throws IOException {
|
|
|
- try (Socket rt = new Socket(ip, port)) {
|
|
|
- rt.setSoTimeout(0);
|
|
|
- rt.setReuseAddress(true);
|
|
|
- rt.setTcpNoDelay(true);
|
|
|
- if (rt.isConnected()) {
|
|
|
- System.out.println("Connected to UR Realtime Client");
|
|
|
- }
|
|
|
- dos = new DataOutputStream(rt.getOutputStream());
|
|
|
- dis = new DataInputStream(rt.getInputStream());
|
|
|
+ public void start() throws IOException, InterruptedException {
|
|
|
+ Thread readThread = new Thread(this);
|
|
|
+ readThread.start();
|
|
|
+ socketOpen.acquire();
|
|
|
+ init();
|
|
|
+ }
|
|
|
|
|
|
- init();
|
|
|
- //Thread readThread = new Thread(this);
|
|
|
- //readThread.start();
|
|
|
- // send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
|
|
|
- }
|
|
|
+ private void request(IRtdeData data) throws IOException, InterruptedException {
|
|
|
+ waitingRequest=data;
|
|
|
+ data.send(dos);
|
|
|
+ waitingRequestSem.acquire();
|
|
|
+ return;
|
|
|
}
|
|
|
|
|
|
// Internal method that actually reads the data
|
|
|
- private void readSocket() throws IOException {
|
|
|
+ private void readSocket() throws IOException, InterruptedException {
|
|
|
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();
|
|
|
+ int length = dis.readShort();
|
|
|
+ int type=dis.readByte();
|
|
|
+ if(type==waitingRequest.getType()) {
|
|
|
+ waitingRequest.read(dis);
|
|
|
+ waitingRequestSem.release();
|
|
|
+ } else switch(type) {
|
|
|
+ case RTDE_TEXT_MESSAGE:
|
|
|
+ var rpv = new RtdeTextMessage();
|
|
|
+ request(rpv);
|
|
|
+ logger.info("RTDE_TEXT_MESSAGE", rpv.text);
|
|
|
+ break;
|
|
|
+
|
|
|
+ default: {
|
|
|
+ logger.warn(String.format("Unexpected package type: %d", type));
|
|
|
+ byte[] buf=new byte[length];
|
|
|
+ dis.readFully(buf);
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- private void receive(RtdeData cmd) throws IOException {
|
|
|
- logger.info(cmd.getType().toString());
|
|
|
- cmd.read(dis);
|
|
|
}
|
|
|
|
|
|
@Override
|
|
|
public void run() {
|
|
|
try {
|
|
|
- init();
|
|
|
- } catch (IOException e) {
|
|
|
- e.printStackTrace();
|
|
|
- }
|
|
|
-
|
|
|
- while (_running) {
|
|
|
- try {
|
|
|
- readSocket();
|
|
|
- } catch (IOException e) {
|
|
|
- e.printStackTrace();
|
|
|
+ Socket rt = new Socket(ip, port);
|
|
|
+ rt.setSoTimeout(0);
|
|
|
+ rt.setReuseAddress(true);
|
|
|
+ rt.setTcpNoDelay(true);
|
|
|
+ if (rt.isConnected()) {
|
|
|
+ System.out.println("Connected to UR Realtime Client");
|
|
|
}
|
|
|
+ dos = new DataOutputStream(rt.getOutputStream());
|
|
|
+ dis = new DataInputStream(rt.getInputStream());
|
|
|
+ socketOpen.release();
|
|
|
+ while (_running) {
|
|
|
+ try {
|
|
|
+ readSocket();
|
|
|
+ } catch (IOException | InterruptedException e) {
|
|
|
+ e.printStackTrace();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } catch (IOException e) {
|
|
|
+ logger.warn("run",e);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- public void send_input_setup(List<String> variables, List<String> types) throws IOException {
|
|
|
- var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
|
|
|
- var payload = String.join(",", variables);
|
|
|
- var rpv = new RtdeRequestProtocolVersion();
|
|
|
- }
|
|
|
-
|
|
|
- private void sendAndReceive(RtdeData cmd) throws IOException {
|
|
|
- cmd.send(dos);
|
|
|
- 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);
|
|
|
bytes.putDouble(frequency);
|
|
|
bytes.put(p);
|
|
|
-
|
|
|
- // sendAndReceive(rpv, bytes.array());
|
|
|
}
|
|
|
|
|
|
- public void init() throws IOException {
|
|
|
+ public void init() throws IOException, InterruptedException {
|
|
|
var rpv = new RtdeRequestProtocolVersion();
|
|
|
- sendAndReceive(rpv);
|
|
|
+ request(rpv);
|
|
|
var ruv = new RtdeRequestURVersion();
|
|
|
- sendAndReceive(ruv);
|
|
|
-
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- private interface RtdeData<T> {
|
|
|
- CommandType getType();
|
|
|
-
|
|
|
- int getSize();
|
|
|
-
|
|
|
- void setReplySize(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 {
|
|
|
- setReplySize(dis.readShort());
|
|
|
- int cmd = dis.readByte();
|
|
|
- if(cmd==getType().getVal())
|
|
|
- return;
|
|
|
- if (cmd==77) {
|
|
|
- int mLength=dis.readByte();
|
|
|
- byte[] mText = new byte[mLength];
|
|
|
- dis.readFully(mText);
|
|
|
- String m=new String(mText);
|
|
|
- logger.info(m);
|
|
|
- int sLength=dis.readByte();
|
|
|
- byte[] sText = new byte[sLength];
|
|
|
- dis.readFully(sText);
|
|
|
- String s=new String(sText);
|
|
|
- logger.info(s);
|
|
|
- int warning=dis.readByte();
|
|
|
- String w=String.valueOf(warning);
|
|
|
-
|
|
|
- } else throw new IOException(String.format("Expected %d, got %d", getType().getVal(), cmd));
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- private class RtdeRequestURVersion implements RtdeData {
|
|
|
- private int replySize;
|
|
|
- private int major, minor, bugfix, build;
|
|
|
-
|
|
|
- @Override
|
|
|
- public CommandType getType() {
|
|
|
- return RTDE_GET_URCONTROL_VERSION;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public int getSize() {
|
|
|
- return 3;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public void setReplySize(int s) {
|
|
|
- replySize = s;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public RtdeRequestURVersion read(DataInputStream di) throws IOException {
|
|
|
- readHeader(di);
|
|
|
- major=di.readInt();
|
|
|
- minor=di.readInt();
|
|
|
- bugfix=di.readInt();
|
|
|
- build=di.readInt();
|
|
|
-
|
|
|
- return this;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
|
|
|
- sendHeader(dos);
|
|
|
- return this;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- private class RtdeRequestProtocolVersion implements RtdeData {
|
|
|
- public boolean success;
|
|
|
- private int replySize;
|
|
|
-
|
|
|
- @Override
|
|
|
- public CommandType getType() {
|
|
|
- return RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public int getSize() {
|
|
|
- return 5;
|
|
|
- }
|
|
|
-
|
|
|
- @Override
|
|
|
- public void setReplySize(int size) {
|
|
|
- this.replySize = 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;
|
|
|
- }
|
|
|
+ request(ruv);
|
|
|
}
|
|
|
}
|