123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228 |
- package at.acdp.urweb.rtde;
- import org.slf4j.LoggerFactory;
- import java.io.DataInputStream;
- import java.io.DataOutputStream;
- import java.io.IOException;
- import java.net.Socket;
- import java.nio.ByteBuffer;
- 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 final 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);
- 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());
- init();
- //Thread readThread = new Thread(this);
- //readThread.start();
- // send_output_setup(List.of(OutParams.timestamp), 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();
- }
- }
- }
- 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();
- }
- }
- }
- 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 {
- var rpv = new RtdeRequestProtocolVersion();
- sendAndReceive(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;
- }
- }
- }
|