|
@@ -1,6 +1,6 @@
|
|
|
package at.acdp.urweb.rtde;
|
|
|
|
|
|
-import net.openhft.chronicle.bytes.Bytes;
|
|
|
+import org.slf4j.LoggerFactory;
|
|
|
|
|
|
import java.io.DataInputStream;
|
|
|
import java.io.DataOutputStream;
|
|
@@ -8,16 +8,19 @@ import java.io.IOException;
|
|
|
import java.net.Socket;
|
|
|
import java.nio.ByteBuffer;
|
|
|
import java.nio.ByteOrder;
|
|
|
-import java.util.ArrayList;
|
|
|
import java.util.List;
|
|
|
|
|
|
+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);
|
|
|
|
|
|
private final String ip;
|
|
|
private final int port;
|
|
|
private volatile boolean _running=true;
|
|
|
private DataOutputStream dos;
|
|
|
private DataInputStream dis;
|
|
|
+ public static int RTDE_PROTOCOL_VERSION=2;
|
|
|
|
|
|
|
|
|
public RTDEClient(String ip, int port) {
|
|
@@ -34,8 +37,9 @@ public class RTDEClient implements Runnable {
|
|
|
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);
|
|
|
+ //readThread.start();
|
|
|
+ negotiate_protocol_version();
|
|
|
+ send_output_setup(List.of(OutParams.timestamp), List.of(), 125);
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -54,6 +58,65 @@ public class RTDEClient implements Runnable {
|
|
|
|
|
|
}
|
|
|
|
|
|
+ private interface RtdeData<T> {
|
|
|
+ public CommandType getType();
|
|
|
+ public T read(DataInputStream di) throws IOException;
|
|
|
+
|
|
|
+ }
|
|
|
+ private class RtdeRequestProtocolVersion implements RtdeData{
|
|
|
+
|
|
|
+ public int major;
|
|
|
+ public int minor;
|
|
|
+ public int bugfix;
|
|
|
+ public int build;
|
|
|
+
|
|
|
+ @Override
|
|
|
+ public CommandType getType() {
|
|
|
+ return RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
+ }
|
|
|
+
|
|
|
+ public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
|
|
|
+ major = d.readInt();
|
|
|
+ minor = d.readInt();
|
|
|
+ bugfix = d.readInt();
|
|
|
+ build = d.readInt();
|
|
|
+ return this;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ private void receive(RtdeData cmd) throws IOException {
|
|
|
+ switch (cmd) {
|
|
|
+ case RTDE_REQUEST_PROTOCOL_VERSION:
|
|
|
+ logger.info("RTDE_REQUEST_PROTOCOL_VERSION");
|
|
|
+ var pv=new ProtocolVersion();
|
|
|
+ return pv.read(dis);
|
|
|
+ break;
|
|
|
+ case RTDE_GET_URCONTROL_VERSION:
|
|
|
+ logger.info("RTDE_GET_URCONTROL_VERSION");
|
|
|
+ break;
|
|
|
+ case RTDE_TEXT_MESSAGE:
|
|
|
+ logger.info("RTDE_TEXT_MESSAGE");
|
|
|
+ break;
|
|
|
+ case RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
|
|
|
+ logger.info("RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS");
|
|
|
+ break;
|
|
|
+ case RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
|
|
|
+ logger.info("RTDE_CONTROL_PACKAGE_SETUP_INPUTS");
|
|
|
+ break;
|
|
|
+ case RTDE_CONTROL_PACKAGE_START:
|
|
|
+ logger.info("RTDE_CONTROL_PACKAGE_START");
|
|
|
+ break;
|
|
|
+ case RTDE_CONTROL_PACKAGE_PAUSE:
|
|
|
+ logger.info("RTDE_CONTROL_PACKAGE_PAUSE");
|
|
|
+ break;
|
|
|
+ case RTDE_DATA_PACKAGE:
|
|
|
+ logger.info("RTDE_DATA_PACKAGE");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ return null;
|
|
|
+ }
|
|
|
+
|
|
|
@Override
|
|
|
public void run() {
|
|
|
while(_running) {
|
|
@@ -68,11 +131,13 @@ public class RTDEClient implements Runnable {
|
|
|
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());
|
|
|
+ var rpv = new RtdeRequestProtocolVersion();
|
|
|
+ sendAndReceive(rpv, payload.getBytes());
|
|
|
}
|
|
|
|
|
|
- private void sendAndReceive(CommandType cmd, byte[] payload) {
|
|
|
+ private void sendAndReceive(RtdeData cmd, byte[] payload) {
|
|
|
sendall(cmd, payload);
|
|
|
+ receive(cmd);
|
|
|
}
|
|
|
|
|
|
|
|
@@ -86,7 +151,16 @@ public class RTDEClient implements Runnable {
|
|
|
sendAndReceive(cmd, bytes.array());
|
|
|
}
|
|
|
|
|
|
- public void sendall(CommandType cmd, byte[] payload) {
|
|
|
+
|
|
|
+ public void negotiate_protocol_version() {
|
|
|
+ var cmd = RTDE_REQUEST_PROTOCOL_VERSION;
|
|
|
+ ByteBuffer bytes = ByteBuffer.allocate(2);
|
|
|
+ bytes.putShort((short) RTDE_PROTOCOL_VERSION);
|
|
|
+ sendAndReceive(cmd, bytes.array());
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ public void sendall(RtdeData cmd, byte[] payload) {
|
|
|
try {
|
|
|
int size = 3 + payload.length;
|
|
|
ByteBuffer bytes = ByteBuffer.allocate(size);
|