Initial commit

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataProcessing/ParseMeasurementData.cs

@@ -0,0 +1,136 @@
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.DataProcessing;
+
+/// <summary>
+/// Parser for measurement data block from UDP packets
+/// Contains scan points with distance, reflectivity, and status flags
+/// Thread-safe implementation
+/// </summary>
+public sealed class ParseMeasurementData
+{
+    private const uint MaxExpectedBeams = 2751;
+    
+    /// <summary>
+    /// Parses the measurement data block from a UDP sequence
+    /// </summary>
+    /// <param name="buffer">Packet buffer containing the data</param>
+    /// <param name="header">Parsed data header (must contain valid block offset/size)</param>
+    /// <param name="derivedValues">Parsed derived values (required for angle calculation)</param>
+    public MeasurementData ParseUdpSequence(PacketBuffer buffer, DataHeader header, DerivedValues derivedValues)
+    {
+        if (buffer == null)
+            throw new ArgumentNullException(nameof(buffer));
+        
+        if (header == null)
+            throw new ArgumentNullException(nameof(header));
+        
+        // Check if measurement data block is enabled
+        if (!CheckIfMeasurementDataIsPublished(header))
+        {
+            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
+        }
+        
+        // Check if header is valid
+        if (header.IsEmpty)
+        {
+            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
+        }
+        
+        // Check if derived values are available (required for angle calculation)
+        if (derivedValues == null || derivedValues.IsEmpty)
+        {
+            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
+        }
+        
+        var bufferData = buffer.GetBuffer();
+        var offset = header.MeasurementDataBlockOffset;
+        
+        // Validate buffer size (at least 4 bytes for number of beams)
+        if (offset + 4 > bufferData.Length)
+        {
+            throw new ArgumentException(
+                $"Buffer too small to contain measurement data block header (offset: {offset}, buffer size: {bufferData.Length})",
+                nameof(buffer));
+        }
+        
+        var span = bufferData.Span.Slice(offset);
+        
+        // Parse number of beams (first 4 bytes, little endian uint32)
+        var numberOfBeams = ReadWriteHelper.ReadUint32LittleEndian(span, 0);
+        
+        // Validate number of beams (safety check)
+        if (numberOfBeams > MaxExpectedBeams)
+        {
+            // Log warning would go here in production
+            return new MeasurementData(0, Array.Empty<ScanPoint>(), isEmpty: true);
+        }
+        
+        // Calculate required buffer size: 4 bytes (number of beams) + numberOfBeams * 4 bytes (per scan point)
+        var requiredSize = 4 + numberOfBeams * 4;
+        if (offset + requiredSize > bufferData.Length)
+        {
+            throw new ArgumentException(
+                $"Buffer too small to contain all measurement data (required: {requiredSize}, available: {bufferData.Length - offset})",
+                nameof(buffer));
+        }
+        
+        // Parse scan points
+        var scanPoints = new List<ScanPoint>((int)numberOfBeams);
+        var currentAngle = derivedValues.StartAngle;
+        var angleDelta = derivedValues.AngularBeamResolution;
+        
+        for (uint i = 0; i < numberOfBeams; i++)
+        {
+            // Each scan point is 4 bytes:
+            // Offset 4 + i*4: Distance (uint16, mm)
+            // Offset 6 + i*4: Reflectivity (uint8, 0-255)
+            // Offset 7 + i*4: Status flags (uint8)
+            //   Bit 0: Valid
+            //   Bit 1: Infinite
+            //   Bit 2: Glare
+            //   Bit 3: Reflector
+            //   Bit 4: Contamination
+            //   Bit 5: Contamination warning
+            
+            var pointOffset = 4 + (int)(i * 4);
+            var distance = ReadWriteHelper.ReadUint16LittleEndian(span, pointOffset);
+            var reflectivity = ReadWriteHelper.ReadUint8LittleEndian(span, pointOffset + 2);
+            var status = ReadWriteHelper.ReadUint8LittleEndian(span, pointOffset + 3);
+            
+            var isValid = (status & 0x01) != 0;
+            var isInfinite = (status & 0x02) != 0;
+            var hasGlare = (status & 0x04) != 0;
+            var isReflector = (status & 0x08) != 0;
+            var isContaminated = (status & 0x10) != 0;
+            var hasContaminationWarning = (status & 0x20) != 0;
+            
+            scanPoints.Add(new ScanPoint(
+                angle: currentAngle,
+                distance: distance,
+                reflectivity: reflectivity,
+                isValid: isValid,
+                isInfinite: isInfinite,
+                hasGlare: hasGlare,
+                isReflector: isReflector,
+                isContaminated: isContaminated,
+                hasContaminationWarning: hasContaminationWarning
+            ));
+            
+            // Advance angle for next point
+            currentAngle += angleDelta;
+        }
+        
+        return new MeasurementData(numberOfBeams, scanPoints, isEmpty: false);
+    }
+    
+    /// <summary>
+    /// Checks if measurement data block is published (enabled)
+    /// </summary>
+    private static bool CheckIfMeasurementDataIsPublished(DataHeader header)
+    {
+        return !(header.MeasurementDataBlockOffset == 0 && header.MeasurementDataBlockSize == 0);
+    }
+}
+