Revize 306eb3ca

Re #8738 - Client - additional messages

deltarobot-vr/Assets/DeltaRobotVr/Client.cs

@@ -38 +38 @@
         private const UInt16 PingId = 0x3004;
         private const UInt16 EotId = 0xF006;
         private const UInt16 CurrentActuatorPositionId = 0x4003;
+        private const UInt16 CurrentDirectionVectorId = 0x4007;
+        private const UInt16 DesiredDirectionVectorId = 0x4008;
+        private const UInt16 CurveId = 0xF009;
 
         private const UInt16 Size8 = 0x0000;
         private const UInt16 Size16 = 0x1000;
@@ -49 +52 @@
         private static readonly byte[] ProtocolMagicValue = Encoding.ASCII.GetBytes("DeltaRVr");
         private const UInt32 ProtocolVersionValue = 1;
 
+        private const UInt32 PointLength = 12;
+
         
         private Thread _thread;
         
@@ -58 +63 @@
         private readonly object _actuatorPositionLock = new object();
         private Single3 _actuatorPosition;
 
+        private readonly object _currentDirectionVectorLock = new object();
+        private Single3 _currentDirectionVector;
+
+        private readonly object _desiredDirectionVectorLock = new object();
+        private Single3 _desiredDirectionVector;
+
+        private readonly object _curveLock = new object();
+        private Single3[] _curve;
+        private long _curveCounter = 0;
+
         private volatile bool _isConnected;
         private volatile string _eotMsg;
 
@@ -112 +127 @@
             } 
         }
 
+        public Single3 CurrentDirectionVector
+        {
+            get
+            {
+                lock (_currentDirectionVectorLock)
+                {
+                    return _currentDirectionVector;
+                }
+            }
+            private set
+            {
+                lock (_currentDirectionVectorLock)
+                {
+                    _currentDirectionVector = value;
+                }
+            }
+        }
+
+        public Single3 DesiredDirectionVector
+        {
+            get
+            {
+                lock (_desiredDirectionVectorLock)
+                {
+                    return _desiredDirectionVector;
+                }
+            }
+            private set
+            {
+                lock (_desiredDirectionVectorLock)
+                {
+                    _desiredDirectionVector = value;
+                }
+            }
+        }
+
+        public Single3[] Curve
+        {
+            get
+            {
+                lock (_curveLock)
+                {
+                    return _curve;
+                }
+            }
+            private set
+            {
+                lock (_curveLock)
+                {
+                    _curve = value;
+                }
+            }
+        }
+
+        public long CurveCounter
+        {
+            get
+            {
+                lock (_curveLock)
+                {
+                    return _curveCounter;
+                }
+            }
+            private set
+            {
+                lock (_curveLock)
+                {
+                    _curveCounter = value;
+                }
+            }
+        }
+
         public bool IsRunning
         {
             get {
@@ -166 +253 @@
                             case CurrentActuatorPositionId:
                                 ProcessCurrentActuatorPosition(reader);
                                 break;
+                            case CurrentDirectionVectorId:
+                                ProcessCurrentDirectionVector(reader);
+                                break;
+                            case DesiredDirectionVectorId:
+                                ProcessDesiredDirectionVector(reader);
+                                break;
+                            case CurveId:
+                                ProcessCurve(reader);
+                                break;
                             default: //client tuto zpravu nezna
                                 var sizeIdentifier = (UInt16) (messageIdentifier & 0xF000);
                                 switch (sizeIdentifier)
@@ -250 +346 @@
             ActuatorPosition = new Single3(x, y, z);
         }
 
+        private void ProcessCurrentDirectionVector(BinaryReader reader)
+        {
+            var x = reader.ReadSingle();
+            var y = reader.ReadSingle();
+            var z = reader.ReadSingle();
+            reader.ReadSingle(); // skip unused
+
+            CurrentDirectionVector = new Single3(x, y, z);
+        }
+
+        private void ProcessDesiredDirectionVector(BinaryReader reader)
+        {
+            var x = reader.ReadSingle();
+            var y = reader.ReadSingle();
+            var z = reader.ReadSingle();
+            reader.ReadSingle(); // skip unused
+
+            DesiredDirectionVector = new Single3(x, y, z);
+        }
+
+        private void ProcessCurve(BinaryReader reader)
+        {
+            UInt32 numberOfBytes = reader.ReadUInt32();
+            UInt32 numberOfPoints = numberOfBytes / PointLength;
+            Single3[] curve = new Single3[numberOfPoints];
+            var x, y, z;
+
+            for(int i = 0; i < numberOfPoints; i++)
+            {
+                x = reader.ReadSingle();
+                y = reader.ReadSingle();
+                z = reader.ReadSingle();
+                curve[i] = new Single3(x, y, z);
+            }
+
+            lock(_curveLock)
+            {
+                CurveCounter++;
+                Curve = curve;
+            }
+        }
+
         private void ProcessEot(BinaryReader reader)
         {
             UInt32 numberOfBytes = reader.ReadUInt32();
@@ -263 +401 @@
             byte[] value = reader.ReadBytes(8);
             if (!value.SequenceEqual(ProtocolMagicValue))
             {
-                IsConnected = false; // mozna vyhodit vyjimku
+                IsConnected = false;
             }
         }
 
@@ -272 +410 @@
             UInt32 value = reader.ReadUInt32();
             if (value != ProtocolVersionValue)
             {
-                IsConnected = false; // mozna vyhodit vyjimku
+                IsConnected = false;
             }
         }