Browse Source

send rtde data

Martin Kunz 2 years ago
parent
commit
d152e3a0ac

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

@@ -1,13 +1,9 @@
 package at.acdp.urweb;
 
-import at.acdp.urweb.fhpp.FHMaster;
 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.RtdeControlStart;
-import at.acdp.urweb.rtde.packets.RtdeSetupInputs;
-import at.acdp.urweb.rtde.packets.RtdeSetupOutputs;
+import at.acdp.urweb.rtde.packets.*;
 import at.acdp.urweb.web.FestoXRServer;
 import com.nmote.nanohttp.NanoServer;
 import com.nmote.xr.HTTPServerEndpoint;
@@ -44,6 +40,9 @@ public class Main {
             var start=new RtdeControlStart();
             r.request(start);
 
+            var pos=new RtdeInDataPackage(si);
+            r.request(pos);
+
 
             FestoXRServer fxrs = new FestoXRServer();
             HTTPServerEndpoint xrs = XR.server(fxrs, FestoXRServer.class);

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

@@ -75,7 +75,7 @@ public class RTDEClient implements Runnable {
                 logger.info(String.format("RTDE_TEXT_MESSAGE: %s", rpv.text));
                 break;
             case RTDE_DATA_PACKAGE:
-                var p = new RtdeDataPackage(outputs);
+                var p = new RtdeOutDataPackage(outputs);
                 p.read(dis, length-3);
                 lastData=p.getPdata();
                 lastData.entrySet().forEach(entry->{

+ 95 - 0
src/main/java/at/acdp/urweb/rtde/packets/RtdeInDataPackage.java

@@ -0,0 +1,95 @@
+package at.acdp.urweb.rtde.packets;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.util.HashMap;
+
+import static at.acdp.urweb.rtde.CommandType.RTDE_DATA_PACKAGE;
+import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
+
+public class RtdeInDataPackage implements IRtdeData {
+    private final RtdeSetupInputs inputs;
+    private HashMap<String, String> pdata=new HashMap<>();
+
+
+    public RtdeInDataPackage(RtdeSetupInputs inputs) {
+        this.inputs=inputs;
+    }
+
+    @Override
+    public int getType() {
+        return RTDE_DATA_PACKAGE;
+    }
+
+    public HashMap<String, String> getPdata() {
+        return pdata;
+    }
+
+
+    @Override
+    public RtdeInDataPackage read(DataInputStream di, int length) throws IOException {
+        int recipeID=di.readByte();
+        return this;
+    }
+
+    @Override
+    public RtdeInDataPackage send(DataOutputStream dos) throws IOException {
+        sendHeader(dos,0);
+        dos.writeByte(inputs.recipe);
+        for(int i=0; i<inputs.variables.size(); i++) {
+            String output = inputs.variables.get(i);
+            String type = inputs.types.get(i);
+            switch (type) {
+                case "BOOL":
+                    dos.writeBoolean(Boolean.parseBoolean(pdata.get(output)));
+                    break;
+                case "UINT8":
+                    dos.writeByte(Byte.parseByte(pdata.get(output)));
+                    break;
+                case "UINT32":
+                case "INT32":
+                    dos.writeInt(Integer.parseInt(pdata.get(output)));
+                    break;
+                case "UINT64":
+                    dos.writeLong(Long.parseLong(pdata.get(output)));
+                    break;
+                case "DOUBLE":
+                    dos.writeDouble(Double.parseDouble(pdata.get(output)));
+                    break;
+                case "VECTOR3D": {
+                    String s=pdata.get(output);
+                    var doubles=s.split(" ");
+                    dos.writeDouble(Double.parseDouble(doubles[0]));
+                    dos.writeDouble(Double.parseDouble(doubles[1]));
+                    dos.writeDouble(Double.parseDouble(doubles[2]));
+                    break;}
+                case "VECTOR6D": {
+                    String s=pdata.get(output);
+                    var doubles=s.split(" ");
+                    dos.writeDouble(Double.parseDouble(doubles[0]));
+                    dos.writeDouble(Double.parseDouble(doubles[1]));
+                    dos.writeDouble(Double.parseDouble(doubles[2]));
+                    dos.writeDouble(Double.parseDouble(doubles[3]));
+                    dos.writeDouble(Double.parseDouble(doubles[4]));
+                    dos.writeDouble(Double.parseDouble(doubles[5]));
+                    break;}
+                case "VECTOR6INT32":
+                case "VECTOR6UINT32": {
+                    String s=pdata.get(output);
+                    var ints=s.split(" ");
+                    dos.writeInt(Integer.parseInt(ints[0]));
+                    dos.writeInt(Integer.parseInt(ints[1]));
+                    dos.writeInt(Integer.parseInt(ints[2]));
+                    dos.writeInt(Integer.parseInt(ints[3]));
+                    dos.writeInt(Integer.parseInt(ints[4]));
+                    dos.writeInt(Integer.parseInt(ints[5]));
+                    break;}
+
+                default:
+                    throw new IllegalStateException("Unexpected value: " + type);
+            }
+        }
+        return this;
+    }
+}

+ 6 - 6
src/main/java/at/acdp/urweb/rtde/packets/RtdeDataPackage.java

@@ -4,22 +4,22 @@ import java.io.DataInputStream;
 import java.io.DataOutputStream;
 import java.io.IOException;
 import java.util.HashMap;
-import java.util.concurrent.ConcurrentHashMap;
 
+import static at.acdp.urweb.rtde.CommandType.RTDE_DATA_PACKAGE;
 import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
 
-public class RtdeDataPackage implements IRtdeData {
+public class RtdeOutDataPackage implements IRtdeData {
     private final RtdeSetupOutputs outputs;
     private HashMap<String, String> pdata=new HashMap<>();
 
 
-    public RtdeDataPackage(RtdeSetupOutputs outputs) {
+    public RtdeOutDataPackage(RtdeSetupOutputs outputs) {
         this.outputs=outputs;
     }
 
     @Override
     public int getType() {
-        return RTDE_TEXT_MESSAGE;
+        return RTDE_DATA_PACKAGE;
     }
 
     public HashMap<String, String> getPdata() {
@@ -28,7 +28,7 @@ public class RtdeDataPackage implements IRtdeData {
 
 
     @Override
-    public RtdeDataPackage read(DataInputStream di, int length) throws IOException {
+    public RtdeOutDataPackage read(DataInputStream di, int length) throws IOException {
         int recipeID=di.readByte();
         for(int i=0; i<outputs.variables.size(); i++) {
             String output=outputs.variables.get(i);
@@ -72,7 +72,7 @@ public class RtdeDataPackage implements IRtdeData {
     }
 
     @Override
-    public RtdeDataPackage send(DataOutputStream dos) throws IOException {
+    public RtdeOutDataPackage send(DataOutputStream dos) throws IOException {
         sendHeader(dos,0);
         return this;
     }