fixed config data length

src/main/java/at/acdp/urweb/CountDataInputStream.java

@@ -0,0 +1,52 @@
+package at.acdp.urweb;
+
+import java.io.DataInputStream;
+import java.io.IOException;
+import java.io.InputStream;
+
+public class CountDataInputStream {
+    private DataInputStream di;
+    private int count;
+    public CountDataInputStream(InputStream in) {
+       di=new DataInputStream(in);
+    }
+
+    public int readInt() throws IOException {
+        count+=4;
+        return di.readInt();
+    }
+    public  long readLong() throws IOException {
+        count+=8;
+        return di.readLong();
+    }
+    public boolean readBoolean() throws IOException {
+        count+=1;
+        return di.readBoolean();
+    }
+
+
+    public byte readByte() throws IOException {
+        count+=1;
+        return di.readByte();
+    }
+
+    public double readDouble() throws IOException {
+        count+=8;
+        return di.readDouble();
+    }
+
+    public void readFully(byte[] b) throws IOException {
+        count+=b.length;
+        di.readFully(b);
+    }
+
+    public float readFloat() throws IOException {
+        count+=4;
+        return di.readFloat();
+    }
+
+
+    public int getCount() {
+        return count;
+    }
+}

src/main/java/at/acdp/urweb/sclient/SecondaryClient.java

@@ -1,5 +1,6 @@
 package at.acdp.urweb.sclient;
 
+import at.acdp.urweb.CountDataInputStream;
 import at.acdp.urweb.sclient.data.*;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
@@ -17,7 +18,7 @@ public class SecondaryClient {
     private volatile boolean _running = true;
     private Socket rt;
     private OutputStream os;
-    private DataInputStream in;
+    private CountDataInputStream in;
 
     public SecondaryClient(String ip, int port) {
         this.ip = ip;
@@ -27,14 +28,14 @@ public class SecondaryClient {
     public VersionMessage connect() throws IOException {
         this.rt = new Socket(ip, port);
         this.os = rt.getOutputStream();
-        this.in = new DataInputStream(rt.getInputStream());
+        this.in = new CountDataInputStream(rt.getInputStream());
         VersionMessage vm = new VersionMessage();
         vm.read(in, -1);
         return vm;
     }
 
-    public void readReply(DataInputStream di) throws IOException {
-
+    public void readReply(CountDataInputStream di) throws IOException {
+        int beforeCount=di.getCount();
         int size = di.readInt(); //4
         int pType = di.readByte() & 0xff; //+1=5
         log.info("ptype: "+pType);
@@ -82,41 +83,46 @@ public class SecondaryClient {
                 byte[] pack=new byte[size-5];
                 di.readFully(pack);
         }
+        int afterCount=di.getCount();
+        int diff=afterCount-beforeCount-size;
+        if(diff!=0) {
+            log.warn("size mismatch: " +diff + "package type: "+pType);
+        }
     }
 
-    private ToolCommInfo readToolCommInfo(DataInputStream di, int size) throws IOException {
+    private ToolCommInfo readToolCommInfo(CountDataInputStream di, int size) throws IOException {
         ToolCommInfo tci=new ToolCommInfo();
         tci.read(di, size);
         return tci;
     }
 
-    private void skip(int pType, DataInputStream di, int size) throws IOException {
+    private void skip(int pType, CountDataInputStream di, int size) throws IOException {
         log.trace("skip data for package type: "+pType);
         byte[] data=new byte[size-5];
         di.readFully(data);
     }
 
 
-    private AdditionalInfo readAdditionalInfo(DataInputStream di, int size) throws IOException {
+    private AdditionalInfo readAdditionalInfo(CountDataInputStream di, int size) throws IOException {
         AdditionalInfo ai=new AdditionalInfo();
         ai.read(di, size);
         return ai;
 
     }
 
-    private ForceModeData readForceModeData(DataInputStream di, int size) throws IOException {
+    private ForceModeData readForceModeData(CountDataInputStream di, int size) throws IOException {
         ForceModeData fmd=new ForceModeData();
         fmd.read(di, size);
         return fmd;
     }
 
-    private ConfigurationData readConfigurationData(DataInputStream di, int size) throws IOException {
+    private ConfigurationData readConfigurationData(CountDataInputStream di, int size) throws IOException {
         ConfigurationData cd=new ConfigurationData();
         cd.read(di, size);
         return cd;
     }
 
-    void readRobotMessage(DataInputStream di, int size) throws IOException {
+    void readRobotMessage(CountDataInputStream di, int size) throws IOException {
         long ts = di.readLong();
         char source = (char) (di.readByte() & 0xFF);
         char robotMessageType = (char) (di.readByte() & 0xFF);
@@ -130,7 +136,7 @@ public class SecondaryClient {
         }
     }
 
-    void readRobotState(DataInputStream di, int size) throws IOException {
+    void readRobotState(CountDataInputStream di, int size) throws IOException {
         long ts = di.readLong();
         char source = (char) (di.readByte() & 0xFF);
         char robotMessageType = (char) (di.readByte() & 0xFF);
@@ -142,19 +148,19 @@ public class SecondaryClient {
         }
     }
 
-    CartesianInfo readCartesianInfo(DataInputStream di, int size) throws IOException {
+    CartesianInfo readCartesianInfo(CountDataInputStream di, int size) throws IOException {
         CartesianInfo ci = new CartesianInfo();
         ci.read(di, size);
         return ci;
     }
 
-    KinematicsInfo readKinemsticsInfo(DataInputStream di, int size) throws IOException {
+    KinematicsInfo readKinemsticsInfo(CountDataInputStream di, int size) throws IOException {
         KinematicsInfo ki = new KinematicsInfo();
         ki.read(di, size);
         return ki;
     }
 
-    ModeData readRobotModeData(DataInputStream di, int size) throws IOException {
+    ModeData readRobotModeData(CountDataInputStream di, int size) throws IOException {
         long ts = di.readLong();
         ModeData md = new ModeData();
         md.realRobotConnected = di.readBoolean();
@@ -175,7 +181,7 @@ public class SecondaryClient {
         return md;
     }
 
-    JointData[] readJointData(DataInputStream di, int size) throws IOException {
+    JointData[] readJointData(CountDataInputStream di, int size) throws IOException {
         int joints = size / 41;
         // byte[] buf=new byte[300];
         // di.readFully(buf);

src/main/java/at/acdp/urweb/sclient/data/AdditionalInfo.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -10,7 +12,7 @@ public class AdditionalInfo implements IRead{
 
 
 
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         freeDriveButtonPressed = di.readBoolean();
         freeDriveButtonEnabled = di.readBoolean();
         ioEnabledFreeDrive = di.readBoolean();

src/main/java/at/acdp/urweb/sclient/data/CartesianInfo.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -10,7 +12,7 @@ public class CartesianInfo implements IRead {
     public double tcpOffsetRX,tcpOffsetRY, tcpOffsetRZ;
 
 
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         x=di.readDouble();
         y=di.readDouble();
         z=di.readDouble();

src/main/java/at/acdp/urweb/sclient/data/ConfigurationData.java

@@ -1,42 +1,52 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
 public class ConfigurationData implements IRead {
 
-    double jointMinLimit;
-    double jointMaxLimit;
-    double jointMaxSpeed;
-    double jointMaxAcceleration;
+    double[] jointMinLimit=new double[6];
+    double[] jointMaxLimit=new double[6];
+    double[] jointMaxSpeed=new double[6];
+    double[] jointMaxAcceleration= new double[6];
     double vJointDefault;
     double aJointDefault;
     double vToolDefault;
     double aToolDefault;
     double eqRadius;
-    double dha;
-    double dhd;
-    double dhAlpha;
-    double dhTheta;
+    double[] dha=new double[6];
+    double[] dhd=new double[6];;
+    double[] dhAlpha=new double[6];;
+    double[] dhTheta=new double[6];;
     int masterboardVersion;
     int controllerBoxType;
     int robotType;
     int robotSubType;
 
-    public void read(DataInputStream di, int size) throws IOException {
-        jointMinLimit = di.readDouble();
-        jointMaxAcceleration = di.readDouble();
-        jointMaxSpeed = di.readDouble();
-        jointMaxAcceleration = di.readDouble();
+    public void read(CountDataInputStream di, int size) throws IOException {
+        for(int i=0; i<jointMinLimit.length; i++)
+            jointMinLimit[i] = di.readDouble();
+        for(int i=0; i<jointMaxAcceleration.length; i++)
+            jointMaxAcceleration[i] = di.readDouble();
+        for(int i=0; i<jointMaxSpeed.length; i++)
+            jointMaxSpeed[i] = di.readDouble();
+        for(int i=0; i<jointMaxAcceleration.length; i++)
+            jointMaxAcceleration[i] = di.readDouble();
         vJointDefault = di.readDouble();
         aJointDefault = di.readDouble();
         vToolDefault = di.readDouble();
         aToolDefault = di.readDouble();
         eqRadius = di.readDouble();
-        dha = di.readDouble();
-        dhd = di.readDouble();
-        dhAlpha = di.readDouble();
-        dhTheta = di.readDouble();
+        for(int i=0;i<dha.length;i++)
+            dha[i] = di.readDouble();
+        for(int i=0;i<dhd.length;i++)
+            dhd[i] = di.readDouble();
+        for(int i=0;i<dhAlpha.length;i++)
+            dhAlpha[i] = di.readDouble();
+        for(int i=0;i<dhTheta.length;i++)
+            dhTheta[i] = di.readDouble();
         masterboardVersion = di.readInt();
         controllerBoxType = di.readInt();
         robotType = di.readInt();

src/main/java/at/acdp/urweb/sclient/data/ForceModeData.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -8,7 +10,7 @@ public class ForceModeData implements IRead{
     public double rx, ry, rz;
     public double robotDexterity;
 
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         x=di.readDouble();
         y=di.readDouble();
         z=di.readDouble();

src/main/java/at/acdp/urweb/sclient/data/IRead.java

@@ -1,8 +1,10 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
 public interface IRead {
-    public void read(DataInputStream di, int size) throws IOException;
+    public void read(CountDataInputStream di, int size) throws IOException;
 }

src/main/java/at/acdp/urweb/sclient/data/JointData.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -16,7 +18,7 @@ public class JointData implements IRead{
 
     //TODO: float/double propably wrong
     //total length 41 bytes
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         qActual=di.readDouble();
         qTarget=di.readDouble();
         qdActual=di.readDouble();

src/main/java/at/acdp/urweb/sclient/data/KinematicsInfo.java

@@ -1,12 +1,14 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
 public class KinematicsInfo implements IRead{
 
     public byte[] internalData;
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         internalData=new byte[size-5];
         di.readFully(internalData);
     }

src/main/java/at/acdp/urweb/sclient/data/ToolCommInfo.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -11,7 +13,7 @@ public class ToolCommInfo implements IRead {
     public float txIdleChars;
 
 
-    public void read(DataInputStream di, int size) throws IOException {
+    public void read(CountDataInputStream di, int size) throws IOException {
         baudrate=di.readInt();
         parity=di.readInt();
         rxIdleChars=di.readFloat();

src/main/java/at/acdp/urweb/sclient/data/VersionMessage.java

@@ -1,5 +1,7 @@
 package at.acdp.urweb.sclient.data;
 
+import at.acdp.urweb.CountDataInputStream;
+
 import java.io.DataInputStream;
 import java.io.IOException;
 
@@ -17,7 +19,7 @@ public class VersionMessage implements IRead {
     public int buildNumber;
     public String buildDate;
 
-    public void read(DataInputStream in, int size) throws IOException {
+    public void read(CountDataInputStream in, int size) throws IOException {
         int msgSize = in.readInt();
         msgType = in.readByte() & 0xff;
         tstamp = in.readLong();
@@ -25,14 +27,14 @@ public class VersionMessage implements IRead {
         robotMsgType = in.readByte();
         int projectNameSize = in.readByte() & 0xff;
         byte[] nameBytes = new byte[projectNameSize];
-        in.read(nameBytes);
+        in.readFully(nameBytes);
         name = new String(nameBytes);
         majorVersion = in.readByte() & 0xff;
         minorVersion = in.readByte() & 0xff;
         bugFixVersion = in.readInt();
         buildNumber = in.readInt();
         byte[] buildDateBytes = new byte[msgSize - (16 + projectNameSize + 10)];
-        in.read(buildDateBytes);
+        in.readFully(buildDateBytes);
         buildDate = new String(buildDateBytes);
     }