Initial commit

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ColaASession.cs

@@ -0,0 +1,285 @@
+using System.Net.Sockets;
+using System.Text;
+
+namespace Sick.ColaB;
+
+/// <summary>
+/// ColaA (ASCII) session manager for handling send, receive and process telegrams
+/// Uses ASCII protocol with STX/ETX framing
+/// </summary>
+public sealed class ColaASession : SopasSession
+{
+    private readonly string _ipAddress;
+    private readonly ushort _port;
+    private readonly int _readTimeoutMs;
+    private readonly int _writeTimeoutMs;
+
+    private System.Net.Sockets.TcpClient? _tcpClient;
+    private System.Net.Sockets.NetworkStream? _networkStream;
+    private readonly Lock _lock = new();
+    private bool _disposed;
+
+    /// <summary>
+    /// Creates a new ColaA session
+    /// </summary>
+    public ColaASession(string ipAddress, ushort port, int readTimeoutMs = 5000, int writeTimeoutMs = 5000)
+    {
+        _ipAddress = ipAddress ?? throw new ArgumentNullException(nameof(ipAddress));
+        _port = port;
+        _readTimeoutMs = readTimeoutMs;
+        _writeTimeoutMs = writeTimeoutMs;
+    }
+
+    /// <summary>
+    /// Gets the protocol type of this session
+    /// </summary>
+    public override string ProtocolType => "ColaA";
+
+    /// <summary>
+    /// Gets whether the session is open
+    /// </summary>
+    public override bool IsOpen
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _tcpClient?.Connected ?? false;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Opens the session (connects to scanner)
+    /// </summary>
+    public override async Task OpenAsync(int connectTimeoutMs, CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaASession));
+
+        if (IsOpen)
+            return;
+
+        var tcpClient = new System.Net.Sockets.TcpClient();
+        try
+        {
+            // Connect with timeout
+            var connectTask = tcpClient.ConnectAsync(_ipAddress, _port);
+            var timeoutTask = Task.Delay(connectTimeoutMs, cancellationToken);
+            var completedTask = await Task.WhenAny(connectTask, timeoutTask);
+
+            if (completedTask == timeoutTask)
+            {
+                tcpClient.Dispose();
+                throw new TimeoutException($"Connection timeout after {connectTimeoutMs}ms");
+            }
+
+            await connectTask;
+
+            var stream = tcpClient.GetStream();
+            stream.ReadTimeout = _readTimeoutMs;
+            stream.WriteTimeout = _writeTimeoutMs;
+
+            lock (_lock)
+            {
+                _tcpClient = tcpClient;
+                _networkStream = stream;
+            }
+        }
+        catch
+        {
+            tcpClient?.Dispose();
+            throw;
+        }
+    }
+
+    /// <summary>
+    /// Closes the session (disconnects from scanner)
+    /// </summary>
+    public override void Close()
+    {
+        if (_disposed)
+            return;
+
+        lock (_lock)
+        {
+            try
+            {
+                _networkStream?.Close();
+                _networkStream?.Dispose();
+            }
+            catch { }
+            _networkStream = null;
+
+            try
+            {
+                _tcpClient?.Close();
+                _tcpClient?.Dispose();
+            }
+            catch { }
+            _tcpClient = null;
+        }
+    }
+
+    /// <summary>
+    /// Sends a SOPAS command and waits for reply (ASCII format)
+    /// Command format: "sRN DeviceIdent", "sMN LMCstartmeas", etc.
+    /// </summary>
+    public override async Task<string> SendCommandAsync(string command, int timeoutMs, CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaASession));
+
+        NetworkStream? stream;
+        lock (_lock)
+        {
+            if (!IsOpen || _networkStream == null || _tcpClient == null || !_tcpClient.Connected)
+                throw new InvalidOperationException("Session is not open");
+
+            stream = _networkStream;
+        }
+
+        // Format SOPAS command (ASCII mode): STX + command + ETX
+        var commandBytes = Encoding.ASCII.GetBytes(command);
+        var message = new List<byte> { 0x02 }; // STX
+        message.AddRange(commandBytes);
+        message.Add(0x03); // ETX
+
+        // Send command
+        await stream.WriteAsync(message.ToArray().AsMemory(0, message.Count), cancellationToken);
+        await stream.FlushAsync(cancellationToken);
+
+        // Read reply with retry logic
+        string reply = string.Empty;
+        int retries = 3;
+
+        for (int i = 0; i < retries; i++)
+        {
+            try
+            {
+                reply = await ReadReplyAsync(stream, timeoutMs, cancellationToken);
+                if (!string.IsNullOrEmpty(reply))
+                    break;
+            }
+            catch (TimeoutException) when (i < retries - 1)
+            {
+                // Retry on timeout
+                await Task.Delay(100, cancellationToken);
+                continue;
+            }
+        }
+
+        if (string.IsNullOrEmpty(reply))
+        {
+            throw new TimeoutException($"No reply received after {retries} attempts");
+        }
+
+        return reply;
+    }
+
+    /// <summary>
+    /// Reads a reply from the scanner (ASCII format)
+    /// </summary>
+    private async Task<string> ReadReplyAsync(NetworkStream stream, int timeoutMs, CancellationToken cancellationToken)
+    {
+        var buffer = new byte[4096];
+        var totalBytes = 0;
+        var startTime = DateTime.UtcNow;
+
+        while (DateTime.UtcNow - startTime < TimeSpan.FromMilliseconds(timeoutMs))
+        {
+            if (stream.DataAvailable)
+            {
+                var bytesRead = await stream.ReadAsync(
+                    buffer.AsMemory(totalBytes, buffer.Length - totalBytes),
+                    cancellationToken);
+
+                if (bytesRead == 0)
+                    break;
+
+                totalBytes += bytesRead;
+
+                // Check if we have complete message (ends with ETX)
+                if (totalBytes > 0 && buffer[totalBytes - 1] == 0x03)
+                    break;
+            }
+            else
+            {
+                await Task.Delay(10, cancellationToken);
+            }
+        }
+
+        if (totalBytes == 0)
+            throw new TimeoutException("No reply from scanner");
+
+        // Parse reply (skip STX, remove ETX)
+        var replyStart = buffer[0] == 0x02 ? 1 : 0;
+        var replyEnd = buffer[totalBytes - 1] == 0x03 ? totalBytes - 1 : totalBytes;
+        var replyLength = replyEnd - replyStart;
+
+        if (replyLength <= 0)
+            return string.Empty;
+
+        return Encoding.ASCII.GetString(buffer, replyStart, replyLength);
+    }
+
+    /// <summary>
+    /// Reads scan data telegram from scanner (for event-based reception)
+    /// </summary>
+    public override async Task<byte[]?> ReceiveTelegramAsync(int timeoutMs, CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaASession));
+
+        NetworkStream? stream;
+        lock (_lock)
+        {
+            if (!IsOpen || _networkStream == null)
+                return null;
+
+            stream = _networkStream;
+        }
+
+        var buffer = new byte[480000]; // Large buffer for scan data
+        var totalBytes = 0;
+        var startTime = DateTime.UtcNow;
+
+        while (DateTime.UtcNow - startTime < TimeSpan.FromMilliseconds(timeoutMs))
+        {
+            if (stream.DataAvailable)
+            {
+                var bytesRead = await stream.ReadAsync(
+                    buffer.AsMemory(totalBytes, buffer.Length - totalBytes),
+                    cancellationToken);
+
+                if (bytesRead == 0)
+                    break;
+
+                totalBytes += bytesRead;
+
+                // Check if we have complete message (ends with ETX)
+                if (totalBytes > 0 && buffer[totalBytes - 1] == 0x03)
+                {
+                    // Return complete telegram (including STX and ETX)
+                    return buffer[..totalBytes];
+                }
+            }
+            else
+            {
+                await Task.Delay(10, cancellationToken);
+            }
+        }
+
+        if (totalBytes == 0)
+            return null;
+
+        // Return partial data if available
+        return buffer[..totalBytes];
+    }
+
+    public override void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Close();
+        _disposed = true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ColaBHelper.cs

@@ -0,0 +1,211 @@
+using System.Text;
+
+namespace Sick.ColaB;
+
+/// <summary>
+/// Helper functions for ColaB protocol encoding/decoding
+/// ColaB is a binary protocol used by older SICK scanners like TiM781s
+/// </summary>
+public static class ColaBHelper
+{
+    /// <summary>
+    /// ColaB frame header: 0x02 0x02 0x02 0x02
+    /// </summary>
+    private static readonly byte[] FrameHeader = [0x02, 0x02, 0x02, 0x02];
+
+    /// <summary>
+    /// Creates a ColaB frame from command data
+    /// Format: [STX STX STX STX] [Length (4 bytes BE)] ['s'] [Command Data] [Checksum (1 byte)]
+    /// </summary>
+    public static byte[] CreateFrame(ReadOnlySpan<byte> commandData)
+    {
+        // Calculate total payload length: length field (4) + 's' (1) + command data + checksum (1)
+        // But length field itself contains: 's' (1) + command data length
+        var payloadLength = 1 + commandData.Length; // 's' + command data
+        
+        // Total frame size: header (4) + length (4) + payload + checksum (1)
+        var totalFrameSize = 4 + 4 + payloadLength + 1;
+        var frame = new byte[totalFrameSize];
+        var pos = 0;
+
+        // Write header (STX STX STX STX)
+        FrameHeader.CopyTo(frame, pos);
+        pos += 4;
+
+        // Write length (4 bytes, Big Endian) - this is the length of payload ('s' + command data)
+        WriteUint32BigEndian(frame, pos, (uint)payloadLength);
+        pos += 4;
+
+        // Write 's' character
+        frame[pos++] = (byte)'s';
+
+        // Write command data
+        commandData.CopyTo(frame.AsSpan(pos));
+        pos += commandData.Length;
+
+        // Calculate checksum (XOR of all bytes from 's' character to end of command data)
+        // According to C++ code: checksum starts from byte 8 (the 's' character)
+        byte checksum = frame[8]; // Start with 's' character (byte 8)
+        for (int i = 9; i < pos; i++)
+        {
+            checksum ^= frame[i];
+        }
+
+        // Write checksum
+        frame[pos++] = checksum;
+
+        return frame;
+    }
+
+    /// <summary>
+    /// Parses a ColaB frame and extracts the command data
+    /// </summary>
+    public static bool TryParseFrame(ReadOnlySpan<byte> frame, out ReadOnlySpan<byte> commandData, out int consumedBytes)
+    {
+        commandData = default;
+        consumedBytes = 0;
+
+        if (frame.Length < 9) // Minimum: header (4) + length (4) + 's' (1)
+            return false;
+
+        // Check header
+        if (frame[0] != 0x02 || frame[1] != 0x02 || frame[2] != 0x02 || frame[3] != 0x02)
+            return false;
+
+        // Read length (Big Endian)
+        var payloadLength = ReadUint32BigEndian(frame, 4);
+        var payloadLengthInt = (int)payloadLength;
+        
+        // Check if we have enough data
+        var totalFrameSize = 4 + 4 + payloadLengthInt + 1; // header + length + payload + checksum
+        if (frame.Length < totalFrameSize)
+            return false;
+
+        // Check 's' character
+        if (frame[8] != (byte)'s')
+            return false;
+
+        // Verify checksum (XOR of all bytes from 's' character to end of command data)
+        // According to C++ code: checksum starts from byte 8 (the 's' character)
+        byte checksum = frame[8]; // Start with 's' character (byte 8)
+        for (int i = 9; i < 8 + payloadLengthInt; i++)
+        {
+            checksum ^= frame[i];
+        }
+
+        if (checksum != frame[8 + payloadLengthInt])
+            return false; // Checksum mismatch
+
+        // Extract command data (skip 's' character)
+        commandData = frame.Slice(9, payloadLengthInt - 1);
+        consumedBytes = totalFrameSize;
+
+        return true;
+    }
+
+    /// <summary>
+    /// Encodes a SOPAS command string to ColaB binary format
+    /// Example: "sRN DeviceIdent" -> binary command data
+    /// </summary>
+    public static byte[] EncodeCommand(string command)
+    {
+        return Encoding.ASCII.GetBytes(command);
+    }
+
+    /// <summary>
+    /// Decodes ColaB binary command data to SOPAS command string
+    /// </summary>
+    public static string DecodeCommand(ReadOnlySpan<byte> data)
+    {
+        return Encoding.ASCII.GetString(data);
+    }
+
+    /// <summary>
+    /// Writes a 32-bit unsigned integer in Big Endian format
+    /// </summary>
+    private static void WriteUint32BigEndian(byte[] buffer, int offset, uint value)
+    {
+        if (offset + 3 < buffer.Length)
+        {
+            buffer[offset + 0] = (byte)((value & 0xff000000) >> 24);
+            buffer[offset + 1] = (byte)((value & 0xff0000) >> 16);
+            buffer[offset + 2] = (byte)((value & 0xff00) >> 8);
+            buffer[offset + 3] = (byte)(value & 0xff);
+        }
+    }
+
+    /// <summary>
+    /// Reads a 32-bit unsigned integer in Big Endian format
+    /// </summary>
+    private static uint ReadUint32BigEndian(ReadOnlySpan<byte> buffer, int offset)
+    {
+        if (offset + 3 < buffer.Length)
+        {
+            return ((uint)buffer[offset + 0] << 24) +
+                   ((uint)buffer[offset + 1] << 16) +
+                   ((uint)buffer[offset + 2] << 8) +
+                   buffer[offset + 3];
+        }
+        return 0;
+    }
+
+    /// <summary>
+    /// Writes an integer value to buffer in Big Endian format
+    /// </summary>
+    public static void WriteIntegerBigEndian(byte[] buffer, ref int pos, uint value, int byteWidth)
+    {
+        for (int i = 0; i < byteWidth; i++)
+        {
+            if (pos < buffer.Length)
+            {
+                buffer[pos + byteWidth - 1 - i] = (byte)((value >> (8 * i)) & 0xff);
+            }
+        }
+        pos += byteWidth;
+    }
+
+    /// <summary>
+    /// Reads an integer value from buffer in Big Endian format
+    /// </summary>
+    public static uint ReadIntegerBigEndian(ReadOnlySpan<byte> buffer, ref int pos, int byteWidth)
+    {
+        uint value = 0;
+        for (int i = 0; i < byteWidth; i++)
+        {
+            if (pos + byteWidth - 1 - i < buffer.Length)
+            {
+                value += (uint)buffer[pos + byteWidth - 1 - i] << (8 * i);
+            }
+        }
+        pos += byteWidth;
+        return value;
+    }
+
+    /// <summary>
+    /// Writes a string to buffer
+    /// </summary>
+    public static void WriteString(byte[] buffer, ref int pos, string value)
+    {
+        var bytes = Encoding.ASCII.GetBytes(value);
+        if (pos + bytes.Length <= buffer.Length)
+        {
+            bytes.CopyTo(buffer, pos);
+            pos += bytes.Length;
+        }
+    }
+
+    /// <summary>
+    /// Reads a string from buffer
+    /// </summary>
+    public static string ReadString(ReadOnlySpan<byte> buffer, ref int pos, int length)
+    {
+        if (pos + length <= buffer.Length)
+        {
+            var result = Encoding.ASCII.GetString(buffer.Slice(pos, length));
+            pos += length;
+            return result;
+        }
+        return string.Empty;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ColaBSession.cs

@@ -0,0 +1,245 @@
+using Sick.ColaB.Commands;
+
+namespace Sick.ColaB;
+
+/// <summary>
+/// ColaB session manager for handling send, receive and process telegrams
+/// </summary>
+public sealed class ColaBSession(TcpClient _tcpClient) : SopasSession
+{
+    private readonly Lock _lock = new();
+    private bool _disposed;
+
+    /// <summary>
+    /// Gets the protocol type of this session
+    /// </summary>
+    public override string ProtocolType => "ColaB";
+
+    public override bool IsOpen
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _tcpClient.IsConnected;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Opens the session (connects to scanner)
+    /// </summary>
+    public void Open(int timeoutMs = 5000)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaBSession));
+
+        if (IsOpen)
+            return;
+
+        _tcpClient.Connect(timeoutMs);
+    }
+
+    /// <summary>
+    /// Opens the session asynchronously (connects to scanner)
+    /// </summary>
+    public override Task OpenAsync(int connectTimeoutMs, CancellationToken cancellationToken = default)
+    {
+        Open(connectTimeoutMs);
+        return Task.CompletedTask;
+    }
+
+    /// <summary>
+    /// Closes the session (disconnects from scanner)
+    /// </summary>
+    public override void Close()
+    {
+        if (_disposed)
+            return;
+
+        _tcpClient.Disconnect();
+    }
+
+    /// <summary>
+    /// Executes a command and waits for reply
+    /// IMPORTANT: This method is thread-safe. Only one command can execute at a time.
+    /// </summary>
+    public void ExecuteCommand(CommandBase command, int timeoutMs = 5000)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaBSession));
+
+        // CRITICAL: Lock to prevent concurrent send/receive operations
+        // Without this lock, multiple threads can send commands simultaneously,
+        // causing responses to get mixed up and frame parsing to fail
+        lock (_lock)
+        {
+            if (!IsOpen)
+                throw new InvalidOperationException("Session is not open");
+
+            // Create ColaB frame from command
+            var commandData = command.GetCommandData();
+            var frame = ColaBHelper.CreateFrame(commandData.Span);
+
+            // Send frame
+            _tcpClient.Send(frame);
+
+            // Receive reply with retry logic
+            byte[]? replyFrame = null;
+            int retries = 3;
+            for (int i = 0; i < retries; i++)
+            {
+                try
+                {
+                    replyFrame = _tcpClient.Receive(timeoutMs);
+                    break;
+                }
+                catch (TimeoutException) when (i < retries - 1)
+                {
+                    // Retry on timeout
+                    System.Threading.Thread.Sleep(100);
+                    continue;
+                }
+            }
+
+            if (replyFrame == null || replyFrame.Length == 0)
+                throw new TimeoutException($"No reply received after {retries} attempts");
+
+            // Parse frame and extract command data
+            if (ColaBHelper.TryParseFrame(replyFrame, out var replyData, out _))
+            {
+                var replyDataArray = replyData.ToArray();
+                command.ProcessReply(replyDataArray);
+            }
+            else
+            {
+                throw new InvalidOperationException($"Failed to parse reply frame. Raw reply: {BitConverter.ToString([.. replyFrame.Take(64)])}");
+            }
+        }
+    }
+
+    /// <summary>
+    /// Sends a SOPAS command and waits for reply (string format)
+    /// </summary>
+    public override async Task<string> SendCommandAsync(string command, int timeoutMs, CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaBSession));
+
+        if (!IsOpen)
+            throw new InvalidOperationException("Session is not open");
+
+        // Parse SOPAS command string and create appropriate CommandBase
+        CommandBase colaBCommand;
+        if (command.StartsWith("sRN ", StringComparison.Ordinal))
+        {
+            // Read command
+            var variableName = command[4..].Trim();
+            colaBCommand = new ReadCommand(variableName);
+        }
+        else if (command.StartsWith("sWN ", StringComparison.Ordinal))
+        {
+            // Write command
+            var parts = command[4..].Split(' ', 2);
+            if (parts.Length == 2)
+            {
+                colaBCommand = new WriteCommand(parts[0], parts[1]);
+            }
+            else
+            {
+                throw new ArgumentException($"Invalid write command format: {command}");
+            }
+        }
+        else if (command.StartsWith("sMN ", StringComparison.Ordinal))
+        {
+            // Method command
+            var parts = command[4..].Split(' ', StringSplitOptions.RemoveEmptyEntries);
+            if (parts.Length > 0)
+            {
+                var methodName = parts[0];
+                var parameters = parts.Skip(1).ToArray();
+                colaBCommand = new MethodCommand(methodName, parameters);
+            }
+            else
+            {
+                throw new ArgumentException($"Invalid method command format: {command}");
+            }
+        }
+        else if (command.StartsWith("sEN ", StringComparison.Ordinal))
+        {
+            // Event command
+            var parts = command[4..].Split(' ', 2);
+            if (parts.Length == 2)
+            {
+                colaBCommand = new WriteCommand(parts[0], parts[1]);
+            }
+            else
+            {
+                throw new ArgumentException($"Invalid event command format: {command}");
+            }
+        }
+        else
+        {
+            throw new ArgumentException($"Unsupported command format: {command}");
+        }
+
+        // Execute command
+        ExecuteCommand(colaBCommand, timeoutMs);
+
+        // Return reply
+        if (!colaBCommand.WasSuccessful)
+            return string.Empty;
+
+        if (colaBCommand is ReadCommand readCmd)
+            return readCmd.ReplyValue ?? string.Empty;
+        else if (colaBCommand is MethodCommand methodCmd)
+            return methodCmd.ReplyValue ?? string.Empty;
+        else if (colaBCommand is WriteCommand)
+            return "OK";
+
+        return string.Empty;
+    }
+
+    /// <summary>
+    /// Executes a read command and returns the command object (allowing access to RawReplyData for binary parsing)
+    /// </summary>
+    public ReadCommand ExecuteReadCommand(string variableName, int timeoutMs = 5000)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaBSession));
+
+        if (!IsOpen)
+            throw new InvalidOperationException("Session is not open");
+
+        var readCmd = new ReadCommand(variableName);
+        ExecuteCommand(readCmd, timeoutMs);
+        return readCmd;
+    }
+
+    /// <summary>
+    /// Receives a telegram from scanner (for event-based reception)
+    /// </summary>
+    public override Task<byte[]?> ReceiveTelegramAsync(int timeoutMs, CancellationToken cancellationToken = default)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(ColaBSession));
+
+        if (!IsOpen)
+            return Task.FromResult<byte[]?>(null);
+
+        try
+        {
+            var frame = _tcpClient.Receive(timeoutMs);
+            return Task.FromResult<byte[]?>(frame);
+        }
+        catch (TimeoutException)
+        {
+            return Task.FromResult<byte[]?>(null);
+        }
+    }
+
+    public override void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Close();
+        _disposed = true;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Commands/CommandBase.cs

@@ -0,0 +1,31 @@
+using System.Text;
+
+namespace Sick.ColaB.Commands;
+
+/// <summary>
+/// Base class for ColaB commands
+/// </summary>
+public abstract class CommandBase(string commandString)
+{
+    private readonly string _commandString = commandString ?? throw new ArgumentNullException(nameof(commandString));
+    private bool _wasSuccessful;
+
+    public ReadOnlyMemory<byte> GetCommandData()
+    {
+        return Encoding.ASCII.GetBytes(_commandString);
+    }
+
+    public bool WasSuccessful
+    {
+        get => _wasSuccessful;
+        protected set => _wasSuccessful = value;
+    }
+
+    public abstract bool ProcessReply(ReadOnlyMemory<byte> replyData);
+
+    /// <summary>
+    /// Gets the command string
+    /// </summary>
+    protected string CommandString => _commandString;
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Commands/MethodCommand.cs

@@ -0,0 +1,60 @@
+using System.Text;
+
+namespace Sick.ColaB.Commands;
+
+/// <summary>
+/// Command to call a method on the scanner (sMN - Method by Name)
+/// </summary>
+public sealed class MethodCommand(string methodName, params string[] parameters) : CommandBase($"sMN {methodName} {string.Join(" ", parameters)}")
+{
+    public string MethodName { get; } = methodName;
+    public string[] Parameters { get; } = parameters;
+
+    public string? ReplyValue { get; private set; }
+
+    public override bool ProcessReply(ReadOnlyMemory<byte> replyData)
+    {
+        // Parse reply: "sAN <methodName> <result>" or "sMA <methodName> <result>" or "sFA <errorCode>" (Error)
+        var replyString = Encoding.ASCII.GetString(replyData.Span);
+        
+        // Check if reply starts with "sFA" (Error) - scanner rejected the command
+        if (replyString.StartsWith("sFA", StringComparison.Ordinal))
+        {
+            // Extract error code if present
+            var errorParts = replyString.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+            var errorInfo = errorParts.Length > 1 ? $" (error code: {string.Join(" ", errorParts.Skip(1))})" : "";
+            WasSuccessful = false;
+            ReplyValue = null;
+            return false;
+        }
+
+        // Check if reply starts with "sAN" (Answer) or "sMA" (Method Answer)
+        if (!replyString.StartsWith("sAN", StringComparison.Ordinal) &&
+            !replyString.StartsWith("sMA", StringComparison.Ordinal))
+        {
+            WasSuccessful = false;
+            return false;
+        }
+
+        // Extract result after method name
+        var parts = replyString.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+        
+        if (parts.Length >= 2 && parts[1] == MethodName)
+        {
+            if (parts.Length > 2)
+            {
+                ReplyValue = string.Join(" ", parts.Skip(2));
+            }
+            else
+            {
+                ReplyValue = string.Empty;
+            }
+            WasSuccessful = true;
+            return true;
+        }
+
+        WasSuccessful = false;
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Commands/ReadCommand.cs

@@ -0,0 +1,71 @@
+using System.Text;
+
+namespace Sick.ColaB.Commands;
+
+/// <summary>
+/// Command to read a value from the scanner (sRN - Read by Name)
+/// </summary>
+public sealed class ReadCommand(string variableName) : CommandBase($"sRN {variableName}")
+{
+    public string VariableName { get; } = variableName;
+
+    public string? ReplyValue { get; private set; }
+    
+    public byte[]? RawReplyData { get; private set; }
+
+    public override bool ProcessReply(ReadOnlyMemory<byte> replyData)
+    {
+        // Store raw reply data for binary parsing
+        RawReplyData = replyData.ToArray();
+
+        // Parse reply: "sAN <variableName> <value>" or "sRA <variableName> <value>"
+        // For binary data, only the header is ASCII, the rest is binary
+        // Try to find where ASCII header ends
+        var asciiHeaderEnd = -1;
+        for (int i = 0; i < replyData.Length && i < 100; i++)
+        {
+            if (replyData.Span[i] == 0 || replyData.Span[i] > 127)
+            {
+                // Found non-ASCII byte, header ends here
+                asciiHeaderEnd = i;
+                break;
+            }
+        }
+
+        // If we found where ASCII ends, only decode that part
+        var headerLength = asciiHeaderEnd > 0 ? asciiHeaderEnd : Math.Min(replyData.Length, 100);
+        var replyString = Encoding.ASCII.GetString(replyData.Span[..headerLength]);
+        
+        // Check if reply starts with "sAN" (Answer) or "sRA" (Read Answer)
+        if (!replyString.StartsWith("sAN", StringComparison.Ordinal) &&
+            !replyString.StartsWith("sRA", StringComparison.Ordinal))
+        {
+            WasSuccessful = false;
+            return false;
+        }
+
+        // Extract value after variable name
+        var parts = replyString.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+        
+        if (parts.Length >= 2 && parts[1] == VariableName)
+        {
+            // For binary data, ReplyValue will contain ASCII header only
+            // The actual binary data is in RawReplyData
+            if (parts.Length >= 3)
+            {
+                ReplyValue = string.Join(" ", parts.Skip(2));
+            }
+            else
+            {
+                // Binary data follows immediately after header
+                ReplyValue = string.Empty;
+            }
+            WasSuccessful = true;
+            return true;
+        }
+
+        WasSuccessful = false;
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Commands/WriteCommand.cs

@@ -0,0 +1,41 @@
+using System.Text;
+
+namespace Sick.ColaB.Commands;
+
+/// <summary>
+/// Command to write a value to the scanner (sWN - Write by Name)
+/// </summary>
+public sealed class WriteCommand(string variableName, string value) : CommandBase($"sWN {variableName} {value}")
+{
+    public string VariableName { get; } = variableName;
+    public string Value { get; } = value;
+
+    public override bool ProcessReply(ReadOnlyMemory<byte> replyData)
+    {
+        // Parse reply: "sWA <variableName>" (Write Acknowledge), "sEA <eventName> <value>" (Event Acknowledge), "sAN <methodName> <result>" (Answer), or "sFA <errorCode>" (Error)
+        var replyString = Encoding.ASCII.GetString(replyData.Span);
+        
+        // Check if reply starts with "sFA" (Error) - this indicates command failed
+        if (replyString.StartsWith("sFA", StringComparison.Ordinal))
+        {
+            // Extract error code if present
+            var parts = replyString.Split(' ', StringSplitOptions.RemoveEmptyEntries);
+            var errorInfo = parts.Length > 1 ? $" (error code: {string.Join(" ", parts.Skip(1))})" : "";
+            WasSuccessful = false;
+            return false;
+        }
+
+        // Check if reply starts with "sWA" (Write Acknowledge), "sEA" (Event Acknowledge), or "sAN" (Answer)
+        if (replyString.StartsWith("sWA", StringComparison.Ordinal) ||
+            replyString.StartsWith("sEA", StringComparison.Ordinal) ||
+            replyString.StartsWith("sAN", StringComparison.Ordinal))
+        {
+            WasSuccessful = true;
+            return true;
+        }
+
+        WasSuccessful = false;
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Parsers/LmdScandataParser.cs

@@ -0,0 +1,416 @@
+using System.Text;
+
+namespace Sick.ColaB.Parsers;
+
+/// <summary>
+/// Parser for SICK scanner LMDscandata telegram
+/// Supports both binary (ColaB) and ASCII (ColaA) formats
+/// </summary>
+public static class LmdScandataParser
+{
+    // Constants for validation
+    private const int MinReasonableDistanceCount = 50;
+    private const int MaxReasonableDistanceCount = 5000;
+
+    /// <summary>
+    /// Parse LMDscandata from binary ColaB format
+    /// This is the primary parser for binary protocol data
+    /// </summary>
+    public static ScanDataResult? ParseBinary(ReadOnlySpan<byte> commandData)
+    {
+        try
+        {
+            // Binary LMDscandata format:
+            // Starts with ASCII header: "sRA LMDscandata " or "sSN LMDscandata "
+            // Then binary data follows:
+            // - Various fields (version, device number, etc.)
+            // - "DIST1" (5 bytes ASCII) followed by:
+            //   - Scale factor (4 bytes double)
+            //   - Scale offset (4 bytes double)
+            //   - Starting angle (4 bytes signed int, units: 1/10000 degree)
+            //   - Angular step (2 bytes unsigned short, units: 1/10000 degree)
+            //   - Number of items (2 bytes, big endian uint16)
+            //   - Distance values (each 2 bytes, big endian uint16, units: mm)
+            // - "RSSI1" (5 bytes ASCII) followed by:
+            //   - Number of items (2 bytes, big endian uint16)
+            //   - RSSI values (each 1 or 2 bytes depending on resolution)
+
+            if (commandData.Length < 50)
+            {
+                return null;
+            }
+
+            // Check if it contains "LMDscandata"
+            var commandStart = Encoding.ASCII.GetString(commandData[..Math.Min(50, commandData.Length)]);
+            if (!commandStart.Contains("LMDscandata", StringComparison.Ordinal))
+            {
+                return null;
+            }
+
+            double startAngleDeg = 0.0;
+            double angularStepDeg = 0.0;
+            double scaleFactor = 1.0;
+            double scaleOffset = 0.0;
+
+            // Find "DIST1" in the data
+            var dist1Pattern = Encoding.ASCII.GetBytes("DIST1");
+            var dist1Index = FindPattern(commandData, dist1Pattern);
+
+            if (dist1Index == -1)
+            {
+                return null;
+            }
+
+            // Parse header fields at fixed offsets after DIST1
+            int headerStart = dist1Index + 5; // Right after "DIST1"
+
+            // Scale factor (4 bytes double, big endian) at offset headerStart
+            if (headerStart + 4 <= commandData.Length)
+            {
+                uint scaleFactorInt = ReadUInt32BigEndian(commandData, headerStart);
+                scaleFactor = BitConverter.ToSingle(BitConverter.GetBytes(scaleFactorInt), 0);
+            }
+
+            // Scale offset (4 bytes double, big endian) at offset headerStart + 4
+            if (headerStart + 8 <= commandData.Length)
+            {
+                uint scaleOffsetInt = ReadUInt32BigEndian(commandData, headerStart + 4);
+                scaleOffset = BitConverter.ToSingle(BitConverter.GetBytes(scaleOffsetInt), 0);
+            }
+
+            // Starting angle (4 bytes signed int, big endian) at offset headerStart + 8
+            if (headerStart + 12 <= commandData.Length)
+            {
+                int startAngleInt = (int)ReadUInt32BigEndian(commandData, headerStart + 8);
+                startAngleDeg = startAngleInt / 10000.0;
+            }
+
+            // Angular step width (2 bytes unsigned short, big endian) at offset headerStart + 12
+            if (headerStart + 14 <= commandData.Length)
+            {
+                ushort angularStepInt = ReadUInt16BigEndian(commandData, headerStart + 12);
+                angularStepDeg = angularStepInt / 10000.0;
+            }
+
+            // Find distance count - try multiple offsets
+            if (!TryFindDistanceCount(commandData, headerStart, out ushort distCount, out int distCountOffset))
+            {
+                return null;
+            }
+
+            // Read distance values (each 2 bytes, big endian, units: mm)
+            var distValuesOffset = distCountOffset + 2;
+            if (distValuesOffset + (distCount * 2) > commandData.Length)
+            {
+                return null;
+            }
+
+            var ranges = new List<double>();
+
+            for (int i = 0; i < distCount; i++)
+            {
+                var offset = distValuesOffset + (i * 2);
+                if (offset + 2 > commandData.Length)
+                {
+                    break;
+                }
+
+                // Read uint16 big endian
+                ushort distValue = ReadUInt16BigEndian(commandData, offset);
+                // Apply scale factor and convert from mm to meters
+                double distanceM = (distValue * scaleFactor + scaleOffset) / 1000.0;
+                ranges.Add(distanceM);
+            }
+
+            // Find "RSSI1" for intensities (optional)
+            var rssi1Pattern = Encoding.ASCII.GetBytes("RSSI1");
+            var rssi1Index = FindPattern(commandData, rssi1Pattern, rssiSearchStart: distValuesOffset + (distCount * 2));
+            var intensities = new List<double>();
+
+            if (rssi1Index != -1)
+            {
+                // Read RSSI count
+                var rssiCountOffset = rssi1Index + 5;
+                if (rssiCountOffset + 2 <= commandData.Length)
+                {
+                    ushort rssiCount = ReadUInt16BigEndian(commandData, rssiCountOffset);
+
+                    // Read RSSI values (usually 1 byte each, but can be 2 bytes for 16-bit resolution)
+                    var rssiValuesOffset = rssiCountOffset + 2;
+
+                    // Try to determine if RSSI is 8-bit or 16-bit
+                    bool use16BitRssi = (rssiValuesOffset + (rssiCount * 2) <= commandData.Length) &&
+                                       (rssiCount == ranges.Count);
+
+                    if (use16BitRssi)
+                    {
+                        // 16-bit RSSI values (big endian)
+                        for (int i = 0; i < rssiCount && i < ranges.Count; i++)
+                        {
+                            ushort rssiValue = ReadUInt16BigEndian(commandData, rssiValuesOffset + (i * 2));
+                            // Normalize to 0-1 range (assuming max value is 65535)
+                            intensities.Add(rssiValue);
+                        }
+                    }
+                    else if (rssiValuesOffset + rssiCount <= commandData.Length)
+                    {
+                        // 8-bit RSSI values
+                        for (int i = 0; i < rssiCount && i < ranges.Count; i++)
+                        {
+                            // Normalize to 0-1 range (assuming max value is 255)
+                            intensities.Add(commandData[rssiValuesOffset + i]);
+                        }
+                    }
+                    else
+                    {
+                        for (int i = 0; i < ranges.Count; i++)
+                        {
+                            intensities.Add(1.0);
+                        }
+                    }
+                }
+            }
+
+            if (ranges.Count == 0)
+            {
+                return null;
+            }
+
+            return new ScanDataResult
+            {
+                Ranges = [.. ranges],
+                Intensities = intensities.Count > 0 ? [.. intensities] : null,
+                StartAngleDeg = startAngleDeg,
+                AngularStepDeg = angularStepDeg,
+                ScaleFactor = scaleFactor,
+                ScaleOffset = scaleOffset,
+                Timestamp = DateTime.UtcNow
+            };
+        }
+        catch
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Parse LMDscandata from ASCII format (via ColaB decoding)
+    /// This is a fallback parser when binary parsing fails
+    /// </summary>
+    public static ScanDataResult? ParseAscii(ReadOnlySpan<byte> commandData)
+    {
+        try
+        {
+            // Decode command string to check if it's scan data
+            var commandString = ColaBHelper.DecodeCommand(commandData);
+
+            // Check if this is scan data telegram
+            if (!commandString.Contains("LMDscandata", StringComparison.Ordinal))
+            {
+                return null;
+            }
+
+            return ParseAsciiString(commandString);
+        }
+        catch
+        {
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Parse LMDscandata from ASCII string format
+    /// Format: "sSN LMDscandata <version> <device> ... DIST1 <count> <distances> ... RSSI1 <count> <intensities> ..."
+    /// </summary>
+    public static ScanDataResult? ParseAsciiString(string telegram)
+    {
+        try
+        {
+            // Split telegram into fields (space-separated)
+            var fields = telegram.Split([' '], StringSplitOptions.RemoveEmptyEntries);
+
+            if (fields.Length < 20)
+            {
+                return null;
+            }
+
+            var ranges = new List<double>();
+            var intensities = new List<double>();
+
+            int distIndex = -1;
+            int rssiIndex = -1;
+
+            // Find DIST1 field
+            for (int i = 0; i < fields.Length; i++)
+            {
+                if (fields[i].StartsWith("DIST", StringComparison.Ordinal))
+                {
+                    distIndex = i;
+                    break;
+                }
+            }
+
+            // Find RSSI1 field
+            for (int i = 0; i < fields.Length; i++)
+            {
+                if (fields[i].StartsWith("RSSI", StringComparison.Ordinal))
+                {
+                    rssiIndex = i;
+                    break;
+                }
+            }
+
+            // Parse distances
+            if (distIndex >= 0 && distIndex + 1 < fields.Length)
+            {
+                // Next field after DIST1 is the count (in hex)
+                if (int.TryParse(fields[distIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int distCount))
+                {
+                    // Parse distance values (in hex, units are mm, convert to meters)
+                    for (int i = 0; i < distCount && distIndex + 2 + i < fields.Length; i++)
+                    {
+                        if (int.TryParse(fields[distIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int distValue))
+                        {
+                            // Convert from mm to meters
+                            ranges.Add(distValue / 1000.0);
+                        }
+                    }
+                }
+            }
+
+            // Parse intensities (RSSI)
+            if (rssiIndex >= 0 && rssiIndex + 1 < fields.Length)
+            {
+                // Next field after RSSI1 is the count (in hex)
+                if (int.TryParse(fields[rssiIndex + 1], System.Globalization.NumberStyles.HexNumber, null, out int rssiCount))
+                {
+                    // Parse RSSI values (in hex)
+                    for (int i = 0; i < rssiCount && rssiIndex + 2 + i < fields.Length; i++)
+                    {
+                        if (int.TryParse(fields[rssiIndex + 2 + i], System.Globalization.NumberStyles.HexNumber, null, out int rssiValue))
+                        {
+                            intensities.Add(rssiValue);
+                        }
+                    }
+                }
+            }
+
+            if (ranges.Count == 0)
+            {
+                return null;
+            }
+
+            return new ScanDataResult
+            {
+                Ranges = [.. ranges],
+                Intensities = intensities.Count > 0 ? [.. intensities] : null,
+                StartAngleDeg = 0.0, // ASCII format doesn't include angle info
+                AngularStepDeg = 0.0,
+                ScaleFactor = 1.0,
+                ScaleOffset = 0.0,
+                Timestamp = DateTime.UtcNow
+            };
+        }
+        catch
+        {
+            return null;
+        }
+    }
+
+    #region Helper Methods
+
+    private static int FindPattern(ReadOnlySpan<byte> data, byte[] pattern, int rssiSearchStart = 0)
+    {
+        int searchStart = rssiSearchStart;
+        for (int i = searchStart; i <= data.Length - pattern.Length; i++)
+        {
+            bool found = true;
+            for (int j = 0; j < pattern.Length; j++)
+            {
+                if (data[i + j] != pattern[j])
+                {
+                    found = false;
+                    break;
+                }
+            }
+            if (found)
+            {
+                return i;
+            }
+        }
+        return -1;
+    }
+
+    private static uint ReadUInt32BigEndian(ReadOnlySpan<byte> data, int offset)
+    {
+        if (offset + 4 > data.Length)
+            return 0;
+
+        return (uint)((data[offset] << 24) | (data[offset + 1] << 16) |
+                     (data[offset + 2] << 8) | data[offset + 3]);
+    }
+
+    private static ushort ReadUInt16BigEndian(ReadOnlySpan<byte> data, int offset)
+    {
+        if (offset + 2 > data.Length)
+            return 0;
+
+        return (ushort)((data[offset] << 8) | data[offset + 1]);
+    }
+
+    private static bool TryFindDistanceCount(ReadOnlySpan<byte> commandData, int headerStart,
+        out ushort distCount, out int distCountOffset)
+    {
+        distCount = 0;
+        distCountOffset = -1;
+
+        // Try offset +14 first (matches actual data)
+        int tryOffset1 = headerStart + 14;
+        if (TryReadDistanceCount(commandData, tryOffset1, out distCount, out distCountOffset))
+            return true;
+
+        // Try offset +19 (per C++ reference code)
+        int tryOffset2 = headerStart + 19;
+        if (TryReadDistanceCount(commandData, tryOffset2, out distCount, out distCountOffset))
+            return true;
+
+        // Search for valid count
+        int searchStart = headerStart;
+        int searchEnd = Math.Min(headerStart + 30, commandData.Length - 2);
+
+        for (int offset = searchStart; offset <= searchEnd; offset++)
+        {
+            if (TryReadDistanceCount(commandData, offset, out distCount, out distCountOffset))
+                return true;
+        }
+
+        return false;
+    }
+
+    private static bool TryReadDistanceCount(ReadOnlySpan<byte> commandData, int offset,
+        out ushort count, out int countOffset)
+    {
+        count = 0;
+        countOffset = -1;
+
+        if (offset + 2 > commandData.Length)
+            return false;
+
+        ushort testCount = ReadUInt16BigEndian(commandData, offset);
+
+        if (testCount >= MinReasonableDistanceCount && testCount <= MaxReasonableDistanceCount)
+        {
+            int requiredBytes = offset + 2 + (testCount * 2);
+            if (requiredBytes <= commandData.Length)
+            {
+                count = testCount;
+                countOffset = offset;
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    #endregion
+}

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/README.md

@@ -0,0 +1,156 @@
+# Sick.Tim781s.Colab
+
+Thư viện C# để giao tiếp với SICK TiM781s LiDAR scanner sử dụng ColaB protocol (binary SOPAS).
+
+## Tổng quan
+
+Thư viện này cung cấp giao tiếp với SICK TiM781s scanner thông qua ColaB protocol (binary SOPAS). ColaB là protocol binary được sử dụng bởi các scanner SICK cũ hơn như TiM781s.
+
+## Cấu trúc
+
+- **ColaB/ColaBSession.cs**: Quản lý session và giao tiếp với scanner
+- **Commands/**: Các command classes (ReadCommand, WriteCommand, MethodCommand)
+- **Helpers/ColaBHelper.cs**: Helper functions để encode/decode ColaB frames
+- **Communication/TcpClient.cs**: TCP client implementation
+- **Tim781sScanner.cs**: Main class để sử dụng scanner
+
+## Cách sử dụng
+
+### Kết nối và đọc thông tin cơ bản
+
+```csharp
+using Sick.Tim781s.Colab;
+
+// Tạo scanner instance
+var scanner = new Tim781sScanner("192.168.1.1", 2112);
+
+// Kết nối
+scanner.Connect();
+
+// Đọc device identification
+var deviceIdent = scanner.ReadDeviceIdent();
+Console.WriteLine($"Device: {deviceIdent}");
+
+// Đọc serial number
+var serialNumber = scanner.ReadSerialNumber();
+Console.WriteLine($"Serial: {serialNumber}");
+
+// Đọc firmware version
+var firmwareVersion = scanner.ReadFirmwareVersion();
+Console.WriteLine($"Firmware: {firmwareVersion}");
+
+// Ngắt kết nối
+scanner.Disconnect();
+```
+
+### Thực thi các lệnh tùy chỉnh
+
+```csharp
+// Đọc một biến
+var value = scanner.ExecuteRead("VariableName");
+
+// Ghi một biến
+var success = scanner.ExecuteWrite("VariableName", "value");
+
+// Gọi một method
+var result = scanner.ExecuteMethod("MethodName", "param1", "param2");
+```
+
+### Sử dụng trực tiếp ColaBSession
+
+```csharp
+using Sick.Tim781s.Colab;
+using Sick.Tim781s.Colab.Commands;
+using Sick.Tim781s.Colab.Communication;
+
+// Tạo TCP client và session
+var tcpClient = new TcpClient("192.168.1.1", 2112);
+var session = new ColaBSession(tcpClient);
+
+// Mở session
+session.Open();
+
+// Tạo và thực thi command
+var readCmd = new ReadCommand("DeviceIdent");
+session.ExecuteCommand(readCmd);
+
+if (readCmd.WasSuccessful)
+{
+    Console.WriteLine($"Value: {readCmd.ReplyValue}");
+}
+
+// Đóng session
+session.Close();
+```
+
+## ColaB Protocol Format
+
+ColaB frame format:
+```
+[STX STX STX STX] [Length (4 bytes BE)] ['s'] [Command Data] [Checksum (1 byte)]
+```
+
+- **STX**: 0x02 0x02 0x02 0x02 (4 bytes)
+- **Length**: 4 bytes Big Endian - độ dài của payload ('s' + command data)
+- **'s'**: 1 byte character 's'
+- **Command Data**: SOPAS command string (ASCII)
+- **Checksum**: XOR của tất cả bytes từ length field đến end of command data
+
+## SOPAS Commands
+
+### Read Command (sRN)
+```
+sRN <VariableName>
+Reply: sAN <VariableName> <Value>
+```
+
+### Write Command (sWN)
+```
+sWN <VariableName> <Value>
+Reply: sWA <VariableName>
+```
+
+### Method Command (sMN)
+```
+sMN <MethodName> <Parameters>
+Reply: sAN <MethodName> <Result>
+```
+
+## Ví dụ
+
+### Đọc device information
+```csharp
+var scanner = new Tim781sScanner("192.168.1.1");
+scanner.Connect();
+
+var deviceIdent = scanner.ReadDeviceIdent();
+var serialNumber = scanner.ReadSerialNumber();
+var firmwareVersion = scanner.ReadFirmwareVersion();
+
+Console.WriteLine($"Device: {deviceIdent}");
+Console.WriteLine($"Serial: {serialNumber}");
+Console.WriteLine($"Firmware: {firmwareVersion}");
+```
+
+### Start/Stop measurement
+```csharp
+// Start measurement (mode 1)
+scanner.StartMeasurement();
+
+// Stop measurement (mode 0)
+scanner.StopMeasurement();
+```
+
+## Lưu ý
+
+- ColaB protocol sử dụng Big Endian byte order
+- Checksum được tính bằng XOR của tất cả bytes từ length field đến end of command data
+- Tất cả commands phải được đóng gói trong ColaB frame format
+- Scanner mặc định sử dụng port 2112
+
+## Tham khảo
+
+- SICK TiM781s Manual
+- SOPAS Protocol Documentation
+- sick_scan_xd C++ implementation
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ScanDataResult.cs

@@ -0,0 +1,48 @@
+namespace Sick.ColaB;
+
+/// <summary>
+/// Result of parsing LMDscandata from SICK scanner
+/// Contains raw scan data that can be converted to application-specific formats
+/// </summary>
+public sealed class ScanDataResult
+{
+    /// <summary>
+    /// Distance measurements in meters
+    /// </summary>
+    public double[] Ranges { get; init; } = [];
+
+    /// <summary>
+    /// Intensity/RSSI values (normalized 0-1), optional
+    /// </summary>
+    public double[]? Intensities { get; init; }
+
+    /// <summary>
+    /// Starting angle in degrees
+    /// </summary>
+    public double StartAngleDeg { get; init; }
+
+    /// <summary>
+    /// Angular step width in degrees
+    /// </summary>
+    public double AngularStepDeg { get; init; }
+
+    /// <summary>
+    /// Scale factor applied to distance values
+    /// </summary>
+    public double ScaleFactor { get; init; } = 1.0;
+
+    /// <summary>
+    /// Scale offset applied to distance values
+    /// </summary>
+    public double ScaleOffset { get; init; }
+
+    /// <summary>
+    /// Timestamp when data was parsed
+    /// </summary>
+    public DateTime Timestamp { get; init; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// Checks if the scan data is valid (has range measurements)
+    /// </summary>
+    public bool IsValid => Ranges.Length > 0;
+}

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ScannerClient.cs

@@ -0,0 +1,528 @@
+using Sick.ColaB.Commands;
+using Sick.ColaB.Parsers;
+using System.Net.Sockets;
+using System.Text;
+
+namespace Sick.ColaB;
+
+/// <summary>
+/// High-level client for communicating with SICK scanners
+/// Supports both ColaB (binary) and ColaA (ASCII) protocols with auto-detection
+/// </summary>
+public sealed class ScannerClient : IDisposable
+{
+    private readonly string _ipAddress;
+    private readonly ushort __port;
+    private readonly string _protocol;
+    private readonly int __connectTimeoutMs;
+    private readonly int __commandTimeoutMs;
+    private readonly int __readTimeoutMs;
+
+    private readonly bool _initialProtocolPreference; // Initial protocol preference from constructor
+    private bool _useBinaryProtocol; // Current protocol in use
+
+    // SOPAS session (handles both ColaA and ColaB)
+    private SopasSession? _session;
+    private TcpClient? _colaBTcpClient; // Only for ColaB
+
+    private readonly SemaphoreSlim _connectionLock = new(1, 1);
+    private bool _disposed;
+
+    /// <summary>
+    /// Creates a new scanner client
+    /// </summary>
+    /// <param name="ipAddress">Scanner IP address</param>
+    /// <param name="port">Scanner port (default: 2112)</param>
+    /// <param name="protocol">Protocol to use: "ColaB", "ColaA", or "Auto" (default: "Auto")</param>
+    /// <param name="connectTimeoutMs">Connection timeout in milliseconds (default: 5000)</param>
+    /// <param name="commandTimeoutMs">Command timeout in milliseconds (default: 5000)</param>
+    /// <param name="readTimeoutMs">Read timeout in milliseconds (default: 5000)</param>
+    public ScannerClient(
+        string ipAddress,
+        ushort port = 2112,
+        string protocol = "Auto",
+        int connectTimeoutMs = 5000,
+        int commandTimeoutMs = 5000,
+        int readTimeoutMs = 5000)
+    {
+        if (string.IsNullOrWhiteSpace(ipAddress))
+            throw new ArgumentException("IP address cannot be null or empty", nameof(ipAddress));
+        if (connectTimeoutMs <= 0)
+            throw new ArgumentOutOfRangeException(nameof(connectTimeoutMs), "Timeout must be greater than 0");
+        if (commandTimeoutMs <= 0)
+            throw new ArgumentOutOfRangeException(nameof(commandTimeoutMs), "Timeout must be greater than 0");
+        if (readTimeoutMs <= 0)
+            throw new ArgumentOutOfRangeException(nameof(readTimeoutMs), "Timeout must be greater than 0");
+
+        _ipAddress = ipAddress;
+        __port = port;
+        _protocol = protocol;
+        __connectTimeoutMs = connectTimeoutMs;
+        __commandTimeoutMs = commandTimeoutMs;
+        __readTimeoutMs = readTimeoutMs;
+
+        _initialProtocolPreference = protocol switch
+        {
+            "ColaB" => true,
+            "ColaA" => false,
+            "Auto" => true, // Default to ColaB for Auto mode
+            _ => throw new ArgumentException($"Invalid protocol '{protocol}'. Use 'ColaB', 'ColaA', or 'Auto'", nameof(protocol))
+        };
+
+        _useBinaryProtocol = _initialProtocolPreference;
+    }
+
+    /// <summary>
+    /// Gets whether the client is currently connected
+    /// </summary>
+    public bool IsConnected => _session?.IsOpen ?? false;
+
+    /// <summary>
+    /// Gets the protocol currently in use
+    /// </summary>
+    public string ActualProtocol => _session?.ProtocolType ?? (_useBinaryProtocol ? "ColaB" : "ColaA");
+
+    /// <summary>
+    /// Connects to the scanner with auto protocol detection
+    /// </summary>
+    public async Task ConnectAsync(CancellationToken cancellationToken = default)
+    {
+        await _connectionLock.WaitAsync(cancellationToken);
+        try
+        {
+            if (IsConnected)
+            {
+                return;
+            }
+
+            bool connected = false;
+            // Use initial protocol preference for Auto mode to ensure consistent behavior
+            bool tryColaBFirst = _protocol == "Auto" ? _initialProtocolPreference : (_protocol == "ColaB");
+            
+            // Try first protocol
+            try
+            {
+                if (tryColaBFirst)
+                {
+                    await ConnectColaBAsync();
+                }
+                else
+                {
+                    await ConnectAsciiAsync(cancellationToken);
+                }
+
+                // Set protocol after successful connection (no verification needed like original code)
+                _useBinaryProtocol = tryColaBFirst;
+                connected = true;
+            }
+            catch (Exception)
+            {
+                // First protocol failed, will try fallback if Auto mode
+                Disconnect();
+            }
+
+            // Try fallback protocol if Auto mode
+            if (!connected && _protocol == "Auto")
+            {
+                try
+                {
+                    bool tryColaBSecond = !tryColaBFirst;
+
+                    if (tryColaBSecond)
+                    {
+                        await ConnectColaBAsync();
+                    }
+                    else
+                    {
+                        await ConnectAsciiAsync(cancellationToken);
+                    }
+
+                    // Set protocol after successful connection (no verification needed)
+                    _useBinaryProtocol = tryColaBSecond;
+                    connected = true;
+                }
+                catch (Exception fallbackEx)
+                {
+                    Disconnect();
+                    throw new InvalidOperationException(
+                        $"Failed to connect to scanner at {_ipAddress}:{__port} with both ColaA and ColaB protocols",
+                        fallbackEx);
+                }
+            }
+
+            if (!connected)
+            {
+                throw new InvalidOperationException($"Failed to connect to scanner at {_ipAddress}:{__port}");
+            }
+        }
+        finally
+        {
+            _connectionLock.Release();
+        }
+    }
+
+    /// <summary>
+    /// Disconnects from the scanner
+    /// </summary>
+    public void Disconnect()
+    {
+        // Use synchronous wait on semaphore to ensure thread-safety
+        _connectionLock.Wait();
+        try
+        {
+            // Dispose session first, then client
+            if (_session != null)
+            {
+                try
+                {
+                    _session.Dispose();
+                }
+                catch
+                {
+                    // Ignore disposal errors
+                }
+                finally
+                {
+                    _session = null;
+                }
+            }
+
+            if (_colaBTcpClient != null)
+            {
+                try
+                {
+                    _colaBTcpClient.Dispose();
+                }
+                catch
+                {
+                    // Ignore disposal errors
+                }
+                finally
+                {
+                    _colaBTcpClient = null;
+                }
+            }
+
+            // Reset protocol to initial preference for consistent reconnection behavior
+            _useBinaryProtocol = _initialProtocolPreference;
+        }
+        finally
+        {
+            _connectionLock.Release();
+        }
+    }
+
+    /// <summary>
+    /// Reads the device identification from the scanner
+    /// </summary>
+    public Task<string> ReadDeviceIdentAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sRN DeviceIdent", cancellationToken);
+
+    /// <summary>
+    /// Reads the current device state from the scanner
+    /// </summary>
+    public Task<string> ReadDeviceStateAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sRN SCdevicestate", cancellationToken);
+
+    /// <summary>
+    /// Reads the scan data configuration from the scanner
+    /// </summary>
+    public Task<string> ReadScanDataConfigAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sRN LMDscandatacfg", cancellationToken);
+
+    /// <summary>
+    /// Sets the access mode for the scanner
+    /// </summary>
+    /// <param name="level">Access level (3 = Authorized Client, 4 = Service)</param>
+    /// <param name="passwordHash">Password hash (hex string)</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    public Task<string> SetAccessModeAsync(int level, string passwordHash, CancellationToken cancellationToken = default)
+        => SendCommandAsync($"sMN SetAccessMode {level} {passwordHash}", cancellationToken);
+
+    /// <summary>
+    /// Starts the measurement (scanning) process
+    /// </summary>
+    public Task<string> StartMeasurementAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sMN LMCstartmeas", cancellationToken);
+
+    /// <summary>
+    /// Stops the measurement (scanning) process
+    /// </summary>
+    public Task<string> StopMeasurementAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sMN LMCstopmeas", cancellationToken);
+
+    /// <summary>
+    /// Applies settings and runs the scanner
+    /// </summary>
+    public Task<string> RunAsync(CancellationToken cancellationToken = default)
+        => SendCommandAsync("sMN Run", cancellationToken);
+
+    /// <summary>
+    /// Enables or disables scan data events (ColaA only - ColaB requires polling)
+    /// </summary>
+    /// <param name="enable">True to enable events, false to disable</param>
+    public Task<string> EnableScanDataEventsAsync(bool enable, CancellationToken cancellationToken = default)
+        => SendCommandAsync($"sEN LMDscandata {(enable ? 1 : 0)}", cancellationToken);
+
+    /// <summary>
+    /// Sends a SOPAS command and returns the reply (internal use)
+    /// </summary>
+    private async Task<string> SendCommandAsync(string command, CancellationToken cancellationToken = default)
+    {
+        // Capture session reference to avoid race condition with Disconnect
+        SopasSession? session;
+        bool useBinary;
+
+        await _connectionLock.WaitAsync(cancellationToken);
+        try
+        {
+            if (!IsConnected || _session == null)
+                throw new InvalidOperationException("Not connected to scanner");
+
+            session = _session;
+            useBinary = _useBinaryProtocol;
+        }
+        finally
+        {
+            _connectionLock.Release();
+        }
+
+        // Execute command outside the connection lock to allow concurrent commands
+        string reply;
+        if (useBinary)
+        {
+            reply = await session.SendCommandAsync(command, __commandTimeoutMs, cancellationToken);
+        }
+        else
+        {
+            reply = await session.SendCommandAsync(command, __commandTimeoutMs, cancellationToken);
+        }
+
+        return reply;
+    }
+
+    /// <summary>
+    /// Reads scan data from the scanner
+    /// </summary>
+    public async Task<ScanDataResult?> ReadScanDataAsync(CancellationToken cancellationToken = default)
+    {
+        // Capture session reference to avoid race condition with Disconnect
+        SopasSession? session;
+        bool useBinary;
+
+        await _connectionLock.WaitAsync(cancellationToken);
+        try
+        {
+            if (!IsConnected || _session == null)
+                throw new InvalidOperationException("Not connected to scanner");
+
+            session = _session;
+            useBinary = _useBinaryProtocol;
+        }
+        finally
+        {
+            _connectionLock.Release();
+        }
+
+        // Execute command outside the connection lock to allow concurrent commands
+        // Poll for scan data using "sRN LMDscandata" command
+        // (TiM781S in ColaB mode requires polling - scanner doesn't send automatically)
+
+        if (useBinary && session is ColaBSession colaBSession)
+        {
+            // For ColaB (binary): Get raw binary data and parse as binary
+            var readCmd = colaBSession.ExecuteReadCommand("LMDscandata", __commandTimeoutMs);
+
+            if (readCmd.WasSuccessful && readCmd.RawReplyData != null)
+            {
+                // Use binary parser for ColaB data
+                var result = LmdScandataParser.ParseBinary(readCmd.RawReplyData);
+                if (result != null && result.IsValid)
+                {
+                    return result;
+                }
+            }
+        }
+        else
+        {
+            // For ColaA (ASCII): Get string reply and parse as ASCII
+            var reply = await session.SendCommandAsync("sRN LMDscandata", __commandTimeoutMs, cancellationToken);
+
+            if (!string.IsNullOrEmpty(reply))
+            {
+                var result = LmdScandataParser.ParseAsciiString(reply);
+                if (result != null && result.IsValid)
+                {
+                    return result;
+                }
+            }
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Initializes the scanner (authentication, start measurement, enable scan data)
+    /// </summary>
+    /// <returns>True if initialization completed successfully, false otherwise</returns>
+    public async Task<bool> InitializeAsync(string scannerType = "sick_tim_7xxS", CancellationToken cancellationToken = default)
+    {
+        if (!IsConnected)
+            throw new InvalidOperationException("Not connected to scanner");
+
+
+        bool authSuccess;
+        bool measurementStarted;
+        bool scanDataEnabled;
+
+        // Set access mode (authentication)
+        try
+        {
+            string passwordHash = scannerType.Contains("7xxS", StringComparison.OrdinalIgnoreCase) ||
+                                 scannerType.Contains("safety", StringComparison.OrdinalIgnoreCase)
+                ? "6FD62C05" // Safety scanner password
+                : "F4724744"; // Default password
+
+            var reply = await SetAccessModeAsync(3, passwordHash, cancellationToken);
+            authSuccess = !string.IsNullOrEmpty(reply) && !reply.Contains("sFA");
+        }
+        catch (Exception)
+        {
+            // Authentication may not be required for all scanners
+            authSuccess = true; // Assume OK if not required
+        }
+
+        // Check scanner state (optional)
+        try
+        {
+            await ReadDeviceStateAsync(cancellationToken);
+        }
+        catch
+        {
+            // Not critical
+        }
+
+        // Check scan data configuration (optional)
+        try
+        {
+            await ReadScanDataConfigAsync(cancellationToken);
+        }
+        catch
+        {
+            // Not critical
+        }
+
+        // Try to start measurement (critical)
+        // Note: TiM781S may already be in measurement mode (error sFA 00-01 means already measuring - acceptable)
+        try
+        {
+            var reply = await StartMeasurementAsync(cancellationToken);
+
+            // Check if command succeeded OR if error is "already measuring" (which is acceptable)
+            if (!string.IsNullOrEmpty(reply) && !reply.Contains("sFA"))
+            {
+                measurementStarted = true;
+            }
+            else
+            {
+                // Command returned empty or error - scanner might already be measuring
+                // TiM781S often starts in measurement mode - assume success
+                measurementStarted = true;
+            }
+        }
+        catch (Exception)
+        {
+            // Exception might mean scanner doesn't support this command - assume OK
+            measurementStarted = true;
+        }
+
+        // Apply settings (Run) - optional
+        try
+        {
+            await RunAsync(cancellationToken);
+        }
+        catch
+        {
+            // May not be supported
+        }
+
+        // Enable scan data event (optional for ColaB)
+        // Note: TiM781S in ColaB mode does NOT support sEN commands and requires polling instead
+        // The sEN command is only for ColaA mode to enable automatic event transmission
+        try
+        {
+            var reply = await EnableScanDataEventsAsync(true, cancellationToken);
+
+            // Check if command succeeded
+            if (!string.IsNullOrEmpty(reply) && !reply.Contains("sFA"))
+            {
+                scanDataEnabled = true;
+            }
+            else
+            {
+                // Command failed - TiM781S in ColaB mode does NOT support sEN and requires polling
+                // In ColaB mode, we poll for data using "sRN LMDscandata" instead of events
+                scanDataEnabled = true;
+            }
+        }
+        catch (Exception)
+        {
+            // In ColaB mode, we use polling instead of events - assume OK
+            scanDataEnabled = true;
+        }
+
+        // Return success only if critical steps succeeded
+        bool overallSuccess = authSuccess && measurementStarted && scanDataEnabled;
+        return overallSuccess;
+    }
+
+    #region Private Connection Methods
+
+    private async Task ConnectColaBAsync()
+    {
+        _colaBTcpClient = new TcpClient(_ipAddress, __port);
+
+        _session = new ColaBSession(_colaBTcpClient);
+
+        await _session.OpenAsync(__connectTimeoutMs);
+    }
+
+    private async Task ConnectAsciiAsync(CancellationToken cancellationToken)
+    {
+        _session = new ColaASession(_ipAddress, __port, __readTimeoutMs, __commandTimeoutMs);
+
+        await _session.OpenAsync(__connectTimeoutMs, cancellationToken);
+    }
+
+    #endregion
+
+    #region Private Command Methods
+
+    private async Task<string> SendCommandColaBAsync(string command)
+    {
+        if (_session == null || !_session.IsOpen)
+            throw new InvalidOperationException("Session not initialized");
+
+        return await _session.SendCommandAsync(command, __commandTimeoutMs);
+    }
+
+    private async Task<string> SendCommandAsciiAsync(string command, CancellationToken cancellationToken)
+    {
+        if (_session == null || !_session.IsOpen)
+            throw new InvalidOperationException("Session not initialized");
+
+        return await _session.SendCommandAsync(command, __commandTimeoutMs, cancellationToken);
+    }
+
+    #endregion
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Disconnect();
+        _connectionLock?.Dispose();
+        _disposed = true;
+    }
+}

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/Sick.ColaB.csproj

@@ -0,0 +1,10 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net10.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+  
+</Project>
+

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/SopasSession.cs

@@ -0,0 +1,47 @@
+namespace Sick.ColaB;
+
+/// <summary>
+/// Abstract base class for SOPAS protocol sessions
+/// Supports both ColaA (ASCII) and ColaB (Binary) protocols
+/// </summary>
+public abstract class SopasSession : IDisposable
+{
+    /// <summary>
+    /// Gets whether the session is currently open
+    /// </summary>
+    public abstract bool IsOpen { get; }
+
+    /// <summary>
+    /// Gets the protocol type of this session
+    /// </summary>
+    public abstract string ProtocolType { get; }
+
+    /// <summary>
+    /// Opens the session (connects to scanner)
+    /// </summary>
+    public abstract Task OpenAsync(int connectTimeoutMs, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Closes the session (disconnects from scanner)
+    /// </summary>
+    public abstract void Close();
+
+    /// <summary>
+    /// Sends a SOPAS command and waits for reply
+    /// </summary>
+    /// <param name="command">SOPAS command string (e.g., "sRN DeviceIdent", "sMN LMCstartmeas")</param>
+    /// <param name="timeoutMs">Command timeout in milliseconds</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>Reply string from scanner</returns>
+    public abstract Task<string> SendCommandAsync(string command, int timeoutMs, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Receives a telegram from scanner (for event-based reception)
+    /// </summary>
+    public abstract Task<byte[]?> ReceiveTelegramAsync(int timeoutMs, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Disposes the session
+    /// </summary>
+    public abstract void Dispose();
+}

srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/TcpClient.cs

@@ -0,0 +1,221 @@
+using System.Net.Sockets;
+
+namespace Sick.ColaB;
+
+/// <summary>
+/// TCP client implementation for ColaB communication
+/// </summary>
+public sealed class TcpClient(string _serverIp, ushort _serverPort)
+{
+    private System.Net.Sockets.TcpClient? _tcpClient;
+    private NetworkStream? _stream;
+    private readonly Lock _lock = new();
+    private bool _disposed;
+
+    public string ServerIp => _serverIp;
+    public ushort ServerPort => _serverPort;
+
+    public bool IsConnected
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _tcpClient?.Connected == true && _stream != null;
+            }
+        }
+    }
+
+    public void Connect(int timeoutMs = 5000)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(TcpClient));
+
+        var timeoutDuration = TimeSpan.FromMilliseconds(timeoutMs);
+
+        lock (_lock)
+        {
+            if (IsConnected)
+                return;
+
+            _tcpClient?.Dispose();
+            _tcpClient = new System.Net.Sockets.TcpClient();
+        }
+
+        try
+        {
+            var connectResult = _tcpClient!.BeginConnect(_serverIp, _serverPort, null, null);
+            var success = connectResult.AsyncWaitHandle.WaitOne(timeoutDuration);
+
+            if (!success)
+            {
+                _tcpClient.Dispose();
+                _tcpClient = null;
+                throw new TimeoutException($"Connection timeout after {timeoutMs}ms");
+            }
+
+            _tcpClient.EndConnect(connectResult);
+
+            lock (_lock)
+            {
+                _stream = _tcpClient.GetStream();
+                _stream.ReadTimeout = timeoutMs;
+                _stream.WriteTimeout = timeoutMs;
+            }
+        }
+        catch (Exception ex) when (ex is not TimeoutException)
+        {
+            lock (_lock)
+            {
+                _tcpClient?.Dispose();
+                _tcpClient = null;
+            }
+            throw new InvalidOperationException($"Connection failed to {_serverIp}:{_serverPort}", ex);
+        }
+    }
+
+    public void Disconnect()
+    {
+        lock (_lock)
+        {
+            if (!IsConnected)
+                return;
+
+            _stream?.Dispose();
+            _stream = null;
+            _tcpClient?.Close();
+            _tcpClient?.Dispose();
+            _tcpClient = null;
+        }
+    }
+
+    public void Send(ReadOnlyMemory<byte> data)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(TcpClient));
+
+        NetworkStream? stream;
+        lock (_lock)
+        {
+            if (!IsConnected)
+                throw new InvalidOperationException("Cannot send data: not connected");
+
+            stream = _stream;
+        }
+
+        if (stream == null)
+            throw new InvalidOperationException("Stream is null");
+
+        try
+        {
+            stream.Write(data.Span);
+            stream.Flush();
+        }
+        catch (Exception ex)
+        {
+            throw new InvalidOperationException($"Failed to send data to {_serverIp}:{_serverPort}", ex);
+        }
+    }
+
+    public byte[] Receive(int timeoutMs = 5000)
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(TcpClient));
+
+        NetworkStream? stream;
+        lock (_lock)
+        {
+            if (!IsConnected)
+                throw new InvalidOperationException("Cannot receive data: not connected");
+
+            stream = _stream;
+        }
+
+        if (stream == null)
+            throw new InvalidOperationException("Stream is null");
+
+        try
+        {
+            // Set read timeout
+            var originalTimeout = stream.ReadTimeout;
+            stream.ReadTimeout = timeoutMs;
+
+            try
+            {
+                // Read header first (4 bytes STX)
+                var header = new byte[4];
+                var totalRead = 0;
+                while (totalRead < 4)
+                {
+                    var read = stream.Read(header, totalRead, 4 - totalRead);
+                    if (read == 0)
+                        throw new InvalidOperationException("Connection closed by server");
+                    totalRead += read;
+                }
+
+                // Verify header
+                if (header[0] != 0x02 || header[1] != 0x02 || header[2] != 0x02 || header[3] != 0x02)
+                {
+                    throw new InvalidOperationException($"Invalid ColaB frame header: expected 02-02-02-02, got {BitConverter.ToString(header)}");
+                }
+
+                // Read length (4 bytes, Big Endian)
+                var lengthBytes = new byte[4];
+                totalRead = 0;
+                while (totalRead < 4)
+                {
+                    var read = stream.Read(lengthBytes, totalRead, 4 - totalRead);
+                    if (read == 0)
+                        throw new InvalidOperationException("Connection closed by server");
+                    totalRead += read;
+                }
+
+                var payloadLength = ((uint)lengthBytes[0] << 24) | ((uint)lengthBytes[1] << 16) |
+                                    ((uint)lengthBytes[2] << 8) | lengthBytes[3];
+
+                // Read payload + checksum
+                var totalFrameSize = 4 + 4 + (int)payloadLength + 1; // header + length + payload + checksum
+                var frame = new byte[totalFrameSize];
+
+                // Copy header and length
+                header.CopyTo(frame, 0);
+                lengthBytes.CopyTo(frame, 4);
+
+                // Read remaining data
+                totalRead = 8; // Already read header + length
+                while (totalRead < totalFrameSize)
+                {
+                    var read = stream.Read(frame, totalRead, totalFrameSize - totalRead);
+                    if (read == 0)
+                        throw new InvalidOperationException("Connection closed by server");
+                    totalRead += read;
+                }
+
+                return frame;
+            }
+            catch (IOException ex) when (ex.InnerException is SocketException se && se.SocketErrorCode == SocketError.TimedOut)
+            {
+                throw new TimeoutException($"Receive timeout after {timeoutMs}ms", ex);
+            }
+            finally
+            {
+                stream.ReadTimeout = originalTimeout;
+            }
+        }
+        catch (TimeoutException)
+        {
+            throw; // Re-throw timeout exceptions
+        }
+        catch (Exception ex)
+        {
+            throw new InvalidOperationException($"Failed to receive data from {_serverIp}:{_serverPort}", ex);
+        }
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Disconnect();
+        _disposed = true;
+    }
+}
+