Browse Source

rtde: read loop working

Martin Kunz 2 years ago
parent
commit
dbccb29c71

+ 5 - 0
src/main/java/at/acdp/urweb/Main.java

@@ -5,6 +5,7 @@ import at.acdp.urweb.fhpp.FHMasterSim;
 import at.acdp.urweb.fhpp.FHPP;
 import at.acdp.urweb.fhpp.IFHMaster;
 import at.acdp.urweb.rtde.RTDEClient;
+import at.acdp.urweb.rtde.packets.RtdeSetupOutputs;
 import at.acdp.urweb.web.FestoXRServer;
 import com.nmote.nanohttp.NanoServer;
 import com.nmote.xr.HTTPServerEndpoint;
@@ -25,6 +26,10 @@ public class Main {
 
             RTDEClient r=new RTDEClient("127.0.0.1", 30004);
             r.start();
+            var so=new RtdeSetupOutputs()
+                    .addVariable("timestamp")
+                    .addVariable("robot_mode");
+            r.request(so);
 
             FestoXRServer fxrs = new FestoXRServer();
             HTTPServerEndpoint xrs = XR.server(fxrs, FestoXRServer.class);

+ 7 - 15
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -10,10 +10,6 @@ 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.*;
@@ -31,6 +27,7 @@ public class RTDEClient implements Runnable {
     private final boolean _running = true;
     private DataOutputStream dos;
     private DataInputStream dis;
+    private Thread readThread;
 
 
     public RTDEClient(String ip, int port) {
@@ -39,13 +36,16 @@ public class RTDEClient implements Runnable {
     }
 
     public void start() throws IOException, InterruptedException {
-        Thread readThread = new Thread(this);
+        readThread = new Thread(this);
         readThread.start();
         socketOpen.acquire();
         init();
     }
 
-    private void request(IRtdeData data) throws IOException, InterruptedException {
+    public void request(IRtdeData data) throws IOException, InterruptedException {
+        if(readThread.getId()==Thread.currentThread().getId()) {
+            logger.error("request() must not be called from readThread.");
+        }
         waitingRequest=data;
         data.send(dos);
         waitingRequestSem.acquire();
@@ -57,7 +57,7 @@ public class RTDEClient implements Runnable {
         int length = dis.readShort();
         int type=dis.readByte();
         if(type==waitingRequest.getType()) {
-            waitingRequest.read(dis);
+            waitingRequest.read(dis, length-3);
             waitingRequestSem.release();
         } else switch(type) {
             case RTDE_TEXT_MESSAGE:
@@ -99,14 +99,6 @@ public class RTDEClient implements Runnable {
         }
     }
 
-    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);
-    }
-
     public void init() throws IOException, InterruptedException {
         var rpv = new RtdeRequestProtocolVersion();
         request(rpv);

+ 3 - 8
src/main/java/at/acdp/urweb/rtde/packets/IRtdeData.java

@@ -12,17 +12,12 @@ public interface IRtdeData<T> {
 
     int getType();
 
-    int getSize();
-
-    void setReplySize(int i);
-
-
-    T read(DataInputStream di) throws IOException;
+    T read(DataInputStream di, int length) throws IOException;
 
     T send(DataOutputStream dos) throws IOException;
 
-    default void sendHeader(DataOutputStream dos) throws IOException {
-        dos.writeShort(getSize());
+    default void sendHeader(DataOutputStream dos, int length) throws IOException {
+        dos.writeShort(length+3);
         dos.writeByte(getType());
     }
 

+ 2 - 13
src/main/java/at/acdp/urweb/rtde/packets/RtdeRequestProtocolVersion.java

@@ -1,6 +1,5 @@
 package at.acdp.urweb.rtde.packets;
 
-import at.acdp.urweb.rtde.CommandType;
 import at.acdp.urweb.rtde.RTDEClient;
 
 import java.io.DataInputStream;
@@ -22,24 +21,14 @@ public class RtdeRequestProtocolVersion implements IRtdeData {
     }
 
     @Override
-    public int getSize() {
-        return 5;
-    }
-
-    @Override
-    public void setReplySize(int size) {
-        this.replySize = size;
-    }
-
-    @Override
-    public RtdeRequestProtocolVersion read(DataInputStream d) throws IOException {
+    public RtdeRequestProtocolVersion read(DataInputStream d, int length) throws IOException {
         success = d.readBoolean();
         return this;
     }
 
     @Override
     public RtdeRequestProtocolVersion send(DataOutputStream dos) throws IOException {
-        sendHeader(dos);
+        sendHeader(dos, 2);
         dos.writeShort(RTDEClient.RTDE_PROTOCOL_VERSION);
         dos.flush();
         return this;

+ 2 - 15
src/main/java/at/acdp/urweb/rtde/packets/RtdeRequestURVersion.java

@@ -1,8 +1,5 @@
 package at.acdp.urweb.rtde.packets;
 
-import at.acdp.urweb.rtde.CommandType;
-import at.acdp.urweb.rtde.RTDEClient;
-
 import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
@@ -22,17 +19,7 @@ public class RtdeRequestURVersion implements IRtdeData {
     }
 
     @Override
-    public int getSize() {
-        return 3;
-    }
-
-    @Override
-    public void setReplySize(int s) {
-        replySize = s;
-    }
-
-    @Override
-    public RtdeRequestURVersion read(DataInputStream di) throws IOException {
+    public RtdeRequestURVersion read(DataInputStream di, int length) throws IOException {
         major=di.readInt();
         minor=di.readInt();
         bugfix=di.readInt();
@@ -43,7 +30,7 @@ public class RtdeRequestURVersion implements IRtdeData {
 
     @Override
     public RtdeRequestURVersion send(DataOutputStream dos) throws IOException {
-        sendHeader(dos);
+        sendHeader(dos, 0);
         return this;
     }
 }

+ 47 - 0
src/main/java/at/acdp/urweb/rtde/packets/RtdeSetupOutputs.java

@@ -0,0 +1,47 @@
+package at.acdp.urweb.rtde.packets;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
+
+public class RtdeSetupOutputs implements IRtdeData {
+    public int recipe;
+    private List<String> variables=new ArrayList<>();
+    public String typesText;
+
+    public RtdeSetupOutputs() {
+    }
+
+    public RtdeSetupOutputs addVariable(String name) {
+        variables.add(name);
+        return this;
+    }
+
+    @Override
+    public int getType() {
+        return RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
+    }
+
+    @Override
+    public RtdeSetupOutputs read(DataInputStream d, int length) throws IOException {
+        recipe = d.readByte();
+        byte[] text=new byte[length-1];
+        d.readFully(text);
+        typesText=new String(text);
+        return this;
+    }
+
+    @Override
+    public RtdeSetupOutputs send(DataOutputStream dos) throws IOException {
+        var payload = String.join(",", variables).getBytes();
+        sendHeader(dos, payload.length+8);
+        dos.writeDouble(1); //output frequency [1..125]
+        dos.write(payload);
+        dos.flush();
+        return this;
+    }
+}

+ 2 - 13
src/main/java/at/acdp/urweb/rtde/packets/RtdeTextMessage.java

@@ -4,7 +4,6 @@ import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
 
-import static at.acdp.urweb.rtde.CommandType.RTDE_GET_URCONTROL_VERSION;
 import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
 
 public class RtdeTextMessage implements IRtdeData {
@@ -19,17 +18,7 @@ public class RtdeTextMessage implements IRtdeData {
     }
 
     @Override
-    public int getSize() {
-        return 3;
-    }
-
-    @Override
-    public void setReplySize(int s) {
-        replySize = s;
-    }
-
-    @Override
-    public RtdeTextMessage read(DataInputStream di) throws IOException {
+    public RtdeTextMessage read(DataInputStream di, int length) throws IOException {
         int mLength=di.readByte();
         byte[] msgb=new byte[mLength];
         di.readFully(msgb);
@@ -44,7 +33,7 @@ public class RtdeTextMessage implements IRtdeData {
 
     @Override
     public RtdeTextMessage send(DataOutputStream dos) throws IOException {
-        sendHeader(dos);
+        sendHeader(dos,0);
         return this;
     }
 }