Initial commit

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Cola2Session.cs

@@ -0,0 +1,359 @@
+using Sick.SafetyScanners.Cola2.Commands;
+using Sick.SafetyScanners.DataProcessing;
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Exceptions;
+using Sick.SafetyScanners.Helpers;
+using Sick.SafetyScanners.Interfaces;
+using Sick.SafetyScanners.Types;
+
+namespace Sick.SafetyScanners.Cola2;
+
+/// <summary>
+/// COLA2 session manager for handling send, receive and process telegrams
+/// Thread-safe implementation
+/// </summary>
+public sealed class Cola2Session : ICola2Session
+{
+    private readonly ITcpClient _tcpClient;
+    private readonly ParseTcpPacket _packetParser;
+    private uint? _sessionId;
+    private ushort _requestId;
+    private readonly object _lock = new();
+    private bool _disposed;
+
+    public uint? SessionId
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _sessionId;
+            }
+        }
+        private set
+        {
+            lock (_lock)
+            {
+                _sessionId = value;
+            }
+        }
+    }
+
+    public bool IsOpen
+    {
+        get
+        {
+            // Fast path: read without lock first (volatile-like behavior)
+            var sessionId = _sessionId;
+            var isConnected = _tcpClient.IsConnected;
+
+            // Double-check with lock if needed
+            if (!isConnected || !sessionId.HasValue)
+                return false;
+
+            // Verify with lock to ensure consistency
+            lock (_lock)
+            {
+                return _tcpClient.IsConnected && _sessionId.HasValue;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Creates a new COLA2 session
+    /// </summary>
+    public Cola2Session(ITcpClient tcpClient)
+    {
+        _tcpClient = tcpClient ?? throw new ArgumentNullException(nameof(tcpClient));
+        _packetParser = new ParseTcpPacket();
+        _requestId = 0;
+    }
+
+    public ushort GetNextRequestId()
+    {
+        lock (_lock)
+        {
+            return ++_requestId;
+        }
+    }
+
+    public void Open()
+    {
+        if (_disposed)
+            ObjectDisposedException.ThrowIf(_disposed, nameof(Cola2Session));
+
+        // Close existing session first if open (avoid deadlock by not locking)
+        if (IsOpen)
+        {
+            Close();
+        }
+
+        // Connect TCP if not connected
+        if (!_tcpClient.IsConnected)
+        {
+            _tcpClient.Connect(TimeDuration.FromSeconds(5));
+        }
+
+        // Create session command
+        var createSessionCmd = new CreateSessionCommand();
+
+        // Send command without session ID check
+        if (!createSessionCmd.CanBeExecutedWithoutSessionId)
+        {
+            throw new InvalidOperationException("CreateSession command must be executable without session ID");
+        }
+
+        // Manually set request ID
+        createSessionCmd.RequestId = GetNextRequestId();
+        createSessionCmd.SessionId = 0; // No session ID for CreateSession
+
+        // Assemble and send telegram (GỬI CreateSession command)
+        AssembleAndSendTelegram(createSessionCmd);
+
+        // Receive and process response (NHẬN response từ scanner)
+        var response = ReceiveAndProcessResponse(createSessionCmd,
+            TimeDuration.FromSeconds(5));
+
+        // Parse response
+        var parseResult = _packetParser.ParseTcpSequence(response);
+
+        // Process reply
+        createSessionCmd.ProcessReply(parseResult.Data, parseResult.CommandType, parseResult.CommandMode);
+
+        // Check for error code
+        if (parseResult.ErrorCode.HasValue && parseResult.ErrorCode.Value != 0)
+        {
+            throw new CommandException(
+                0,
+                parseResult.RequestId,
+                parseResult.CommandType,
+                parseResult.CommandMode,
+                parseResult.ErrorCode.Value,
+                $"CreateSession failed with error code: 0x{parseResult.ErrorCode.Value:X4}"
+            );
+        }
+
+        if (!createSessionCmd.WasSuccessful)
+        {
+            throw new SessionException("Failed to create COLA2 session");
+        }
+
+        // Extract session ID from packet header (not from reply data)
+        // In C++ reference, session ID is read from packet header (offset 10) by ParseTCPPacket
+        // and set to Command via setCommandValuesFromPacket(), then retrieved via getSessionID()
+        // In C# implementation, we get it directly from parseResult.SessionId
+        if (parseResult.SessionId == 0)
+        {
+            throw new SessionException("Failed to extract session ID from CreateSession reply: Session ID is 0");
+        }
+
+        SessionId = parseResult.SessionId;
+    }
+
+    public void Close()
+    {
+        ObjectDisposedException.ThrowIf(_disposed, nameof(Cola2Session));
+
+        lock (_lock)
+        {
+            if (!IsOpen)
+            {
+                // Already closed
+                return;
+            }
+        }
+
+        try
+        {
+            var closeSessionCmd = new CloseSessionCommand
+            {
+                SessionId = SessionId ?? 0
+            };
+
+            SendCommand(closeSessionCmd, TimeDuration.FromSeconds(5));
+        }
+        catch (Exception)
+        {
+            // Ignore errors during close
+        }
+        finally
+        {
+            SessionId = null;
+            _tcpClient.Disconnect();
+        }
+    }
+
+    public void SendCommand(ICola2Command command, TimeDuration? timeout = null)
+    {
+
+        ObjectDisposedException.ThrowIf(_disposed, nameof(Cola2Session));
+
+        ArgumentNullException.ThrowIfNull(command);
+
+        // Check if session is required
+        lock (_lock)
+        {
+            if (!command.CanBeExecutedWithoutSessionId && !IsOpen)
+            {
+                throw new SessionException("Attempt to send command in closed Cola2 session state");
+            }
+
+            // Set session ID and request ID
+            // In C++ reference: cmd.setSessionID(getSessionID().get_value_or(0));
+            // We must ensure session ID is set correctly before sending
+            if (!_sessionId.HasValue && !command.CanBeExecutedWithoutSessionId)
+            {
+                throw new SessionException($"Cannot send command: Session ID is not set. Session is open: {IsOpen}");
+            }
+
+            command.SessionId = _sessionId ?? 0;
+            command.RequestId = GetNextRequestId();
+
+            // Debug: Verify session ID is set (only for commands that require session)
+            if (!command.CanBeExecutedWithoutSessionId && command.SessionId == 0)
+            {
+                throw new SessionException($"Session ID is 0 for command that requires session. Current session state: IsOpen={IsOpen}, _sessionId={_sessionId}");
+            }
+        }
+
+        // Assemble and send telegram
+        AssembleAndSendTelegram(command);
+
+        // Receive and process response
+        var response = ReceiveAndProcessResponse(command, timeout);
+        // Parse response
+        var parseResult = _packetParser.ParseTcpSequence(response);
+
+        // Check error code FIRST - if error code is present and non-zero, it indicates an error
+        // even if command type/mode is correct (R/A)
+        // Error code format: 0x[VARIABLE_INDEX]00 typically means "variable index not found"
+        if (parseResult.ErrorCode.HasValue && parseResult.ErrorCode.Value != 0)
+        {
+            var errorCode = parseResult.ErrorCode.Value;
+            var errorMessage = GetErrorMessage(errorCode, command);
+            throw new CommandException(
+                SessionId ?? 0,
+                parseResult.RequestId,
+                parseResult.CommandType,
+                parseResult.CommandMode,
+                errorCode,
+                errorMessage
+            );
+        }
+
+        // Process reply (this checks command type/mode and sets WasSuccessful)
+        command.ProcessReply(parseResult.Data, parseResult.CommandType, parseResult.CommandMode);
+
+        // Check if processReply failed (command type/mode mismatch)
+        if (!command.WasSuccessful)
+        {
+            // When command type/mode is wrong, check for error code in data
+            ushort? errorCode = null;
+            if (parseResult.Data.Length >= 2)
+            {
+                // Error code might be at the start of data payload (offset 18 of packet)
+                errorCode = ReadWriteHelper.ReadUint16BigEndian(parseResult.Data.Span, 0);
+            }
+
+            throw new CommandException(SessionId ?? 0,
+                parseResult.RequestId,
+                parseResult.CommandType,
+                parseResult.CommandMode,
+                parseResult.ErrorCode ?? 0,
+                $"Command failed: processReply returned false. Expected 'R'/'A' (0x52/0x41), got 0x{parseResult.CommandType:X2}/0x{parseResult.CommandMode:X2}"
+            );
+        }
+
+        // Command was successful (command type/mode is 'R'/'A')
+        // The 2 bytes at offset 18-19 are part of the data payload (variable index for VariableCommand)
+        // NOT an error code - error code only exists when command fails
+        // No need to check error code when command is acknowledged
+
+        // Command was successful (processReply returned true and no error code or error code matches variable index)
+    }
+
+    private void AssembleAndSendTelegram(ICola2Command command)
+    {
+        if (command is not CommandBase cmdBase)
+            throw new ArgumentException("Command must be derived from CommandBase", nameof(command));
+
+        var telegram = cmdBase.ConstructTelegram();
+        _tcpClient.Send(telegram);
+    }
+
+    private PacketBuffer ReceiveAndProcessResponse(ICola2Command command,
+        TimeDuration? timeout)
+    {
+        var packetMerger = new TcpPacketMerger();
+
+        try
+        {
+            while (!packetMerger.IsComplete)
+            {
+                var packet = _tcpClient.Receive(timeout);
+
+                if (packetMerger.IsEmpty)
+                {
+                    var expectedLength = _packetParser.GetExpectedPacketLength(packet);
+                    packetMerger.SetTargetSize(expectedLength);
+                }
+
+                // AddTcpPacket returns true if complete
+                var isComplete = packetMerger.AddTcpPacket(packet);
+                if (isComplete)
+                {
+                    break; // Exit loop early when complete
+                }
+            }
+
+            return packetMerger.GetDeployedBuffer();
+        }
+        finally
+        {
+            packetMerger.Dispose();
+        }
+    }
+
+    private string GetErrorMessage(ushort errorCode, ICola2Command command)
+    {
+        // Common COLA2 error codes
+        // 0x0D00 = Variable index not found or not supported
+        // 0x0001 = General error
+        // 0x0002 = Invalid parameter
+        // etc.
+
+        var baseMessage = $"Command failed with error code: 0x{errorCode:X4}";
+
+        if (command is VariableCommand varCmd)
+        {
+            baseMessage += $" (Variable Index: 0x{varCmd.VariableIndex:X4})";
+
+            // Common error codes for variable commands
+            if (errorCode == 0x0D00)
+            {
+                baseMessage += " - Variable index not found or not supported by this scanner model";
+            }
+            else if (errorCode == 0x0001)
+            {
+                baseMessage += " - General error";
+            }
+            else if (errorCode == 0x0002)
+            {
+                baseMessage += " - Invalid parameter";
+            }
+        }
+
+        return baseMessage;
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Close();
+        _tcpClient.Dispose();
+        _disposed = true;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/ChangeCommSettingsCommand.cs

@@ -0,0 +1,122 @@
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+using System.Net;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to change communication settings on the sensor
+/// </summary>
+public sealed class ChangeCommSettingsCommand : MethodCommand
+{
+    private readonly CommSettings _settings;
+    
+    public ChangeCommSettingsCommand(CommSettings settings)
+        : base(0x00b0) // Method index for ChangeCommSettings
+    {
+        _settings = settings ?? throw new ArgumentNullException(nameof(settings));
+    }
+    
+    public CommSettings Settings => _settings;
+    
+    public override bool CanBeExecutedWithoutSessionId => true;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        // Base method index (2 bytes) + 28 bytes for settings data
+        var data = new byte[2 + 28];
+        var span = data.AsSpan();
+        
+        // Write base method index (from MethodCommand.AddTelegramData)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 0, MethodIndex);
+        
+        // Write settings data starting at offset 2 (after method index)
+        WriteDataToSpan(span.Slice(2));
+        
+        return data;
+    }
+    
+    private void WriteDataToSpan(Span<byte> span)
+    {
+        // Channel (offset 0)
+        ReadWriteHelper.WriteUint8LittleEndian(span, 0, _settings.Channel);
+        
+        // Skip 3 bytes (offsets 1, 2, 3)
+        
+        // Enabled (offset 4)
+        ReadWriteHelper.WriteUint8LittleEndian(span, 4, (byte)(_settings.Enabled ? 1 : 0));
+        
+        // Interface type (offset 5)
+        ReadWriteHelper.WriteUint8LittleEndian(span, 5, (byte)_settings.EInterfaceType);
+        
+        // Skip 2 bytes (offsets 6, 7)
+        
+        // Host IP (offset 8, 4 bytes, little endian)
+        if (!IPAddress.TryParse(_settings.HostIp, out var ipAddress) || 
+            ipAddress.AddressFamily != System.Net.Sockets.AddressFamily.InterNetwork)
+        {
+            throw new ArgumentException($"Invalid IPv4 address: {_settings.HostIp}", nameof(_settings));
+        }
+        
+        var bytes = ipAddress.GetAddressBytes();
+        if (bytes.Length != 4)
+        {
+            throw new InvalidOperationException($"IPAddress.GetAddressBytes() returned {bytes.Length} bytes, expected 4");
+        }
+
+        // Convert to uint32 (little endian)
+        // IPAddress.GetAddressBytes() returns bytes in network byte order (big endian): [b0, b1, b2, b3]
+        // For little endian uint32, we need: bytes[0] | (bytes[1] << 8) | (bytes[2] << 16) | (bytes[3] << 24)
+        // uint ipUint = (uint)(bytes[0] | (bytes[1] << 8) | (bytes[2] << 16) | (bytes[3] << 24));
+        uint ipUint = (uint)(bytes[3] | (bytes[2] << 8) | (bytes[1] << 16) | (bytes[0] << 24));
+        ReadWriteHelper.WriteUint32LittleEndian(span, 8, ipUint);
+        
+        // Host UDP port (offset 12, 2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 12, _settings.HostUdpPort);
+        
+        // Publishing frequency (offset 14, 2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 14, _settings.PublishingFrequency);
+        
+        // Start angle (offset 16, 4 bytes, little endian, multiplied by 4194304.0)
+        int startAngleInt = (int)(_settings.StartAngle * 4194304.0);
+        ReadWriteHelper.WriteInt32LittleEndian(span, 16, startAngleInt);
+        
+        // End angle (offset 20, 4 bytes, little endian, multiplied by 4194304.0)
+        int endAngleInt = (int)(_settings.EndAngle * 4194304.0);
+        ReadWriteHelper.WriteInt32LittleEndian(span, 20, endAngleInt);
+        
+        // Features (offset 24, 2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 24, _settings.Features);
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // According to C++ reference, this inverts the result from base class
+        // Base class returns true for 'A' + 'I' (Acknowledge)
+        // But ChangeCommSettingsCommand expects different reply format
+        // Let's check for error response: if we get 'E' + 'I' (Error), return false
+        // Otherwise, check base class logic
+        
+        // Error response: 'E' (0x45) and 'I' (0x49)
+        if ((replyCommandType == 0x45 && replyCommandMode == 0x49) ||
+            (replyCommandType == 'E' && replyCommandMode == 'I'))
+        {
+            return false;
+        }
+        
+        // Success: 'A' (0x41) and 'I' (0x49)
+        if ((replyCommandType == 0x41 && replyCommandMode == 0x49) ||
+            (replyCommandType == 'A' && replyCommandMode == 'I'))
+        {
+            return true;
+        }
+        
+        // For ChangeCommSettings, the C++ code inverts the base result
+        // which suggests it might have different error handling
+        // Let's use standard acknowledge check
+        return (replyCommandType == 0x41 && replyCommandMode == 0x49) ||
+               (replyCommandType == 'A' && replyCommandMode == 'I');
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/CloseSessionCommand.cs

@@ -0,0 +1,34 @@
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to close a COLA2 session
+/// </summary>
+public sealed class CloseSessionCommand : CommandBase
+{
+    public CloseSessionCommand() 
+        : base(0x43, 0x58) // 'C' and 'X' in ASCII
+    {
+    }
+    
+    public override bool CanBeExecutedWithoutSessionId => false;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        // Close session command has no additional data
+        return ReadOnlyMemory<byte>.Empty;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // Reply should be 'C' (0x43) and 'A' (0x41) for Acknowledge
+        if ((replyCommandType == 0x43 && replyCommandMode == 0x41) ||
+            (replyCommandType == 'C' && replyCommandMode == 'A'))
+        {
+            return true;
+        }
+        
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/CommandBase.cs

@@ -0,0 +1,201 @@
+using Sick.SafetyScanners.Cola2.Commands;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Base class for all COLA2 commands
+/// Thread-safe implementation
+/// </summary>
+public abstract class CommandBase : ICola2Command
+{
+    private readonly byte _commandType;
+    private readonly byte _commandMode;
+    private uint _sessionId;
+    private ushort _requestId;
+    private bool _wasSuccessful;
+    private readonly List<byte> _dataVector;
+    private readonly object _lock = new();
+    
+    protected CommandBase(byte commandType, byte commandMode)
+    {
+        _commandType = commandType;
+        _commandMode = commandMode;
+        _dataVector = new List<byte>();
+    }
+    
+    public byte CommandType => _commandType;
+    public byte CommandMode => _commandMode;
+    
+    public uint SessionId
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _sessionId;
+            }
+        }
+        set
+        {
+            lock (_lock)
+            {
+                _sessionId = value;
+            }
+        }
+    }
+    
+    public ushort RequestId
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _requestId;
+            }
+        }
+        set
+        {
+            lock (_lock)
+            {
+                _requestId = value;
+            }
+        }
+    }
+    
+    public bool WasSuccessful
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _wasSuccessful;
+            }
+        }
+        protected set
+        {
+            lock (_lock)
+            {
+                _wasSuccessful = value;
+            }
+        }
+    }
+    
+    public abstract bool CanBeExecutedWithoutSessionId { get; }
+    
+    /// <summary>
+    /// Gets the data vector for the command payload
+    /// </summary>
+    public ReadOnlyMemory<byte> GetDataVector()
+    {
+        lock (_lock)
+        {
+            return _dataVector.ToArray();
+        }
+    }
+    
+    /// <summary>
+    /// Sets the data vector
+    /// </summary>
+    protected void SetDataVector(ReadOnlyMemory<byte> data)
+    {
+        lock (_lock)
+        {
+            _dataVector.Clear();
+            _dataVector.AddRange(data.ToArray());
+        }
+    }
+    
+    /// <summary>
+    /// Adds data to the data vector
+    /// </summary>
+    protected void AddData(ReadOnlyMemory<byte> data)
+    {
+        lock (_lock)
+        {
+            _dataVector.AddRange(data.ToArray());
+        }
+    }
+    
+    /// <summary>
+    /// Constructs the complete telegram including header
+    /// </summary>
+    public byte[] ConstructTelegram()
+    {
+        lock (_lock)
+        {
+            var data = AddTelegramData();
+            return AddTelegramHeader(data);
+        }
+    }
+    
+    /// <summary>
+    /// Adds command-specific data to the telegram
+    /// </summary>
+    protected abstract ReadOnlyMemory<byte> AddTelegramData();
+    
+    /// <summary>
+    /// Processes the reply from the sensor
+    /// In C++ reference, ParseTCPPacket::parseTCPSequence calls command.setDataVector(byte_vector)
+    /// to store the reply data in the command. We need to do the same here.
+    /// </summary>
+    public bool ProcessReply(ReadOnlyMemory<byte> replyData, byte replyCommandType, byte replyCommandMode)
+    {
+        lock (_lock)
+        {
+            // Store reply data in _dataVector (matching C++ reference behavior)
+            // In C++: command.setDataVector(byte_vector) is called by ParseTCPPacket
+            SetDataVector(replyData);
+            
+            _wasSuccessful = ProcessReplyInternal(replyData, replyCommandType, replyCommandMode);
+            return _wasSuccessful;
+        }
+    }
+    
+    /// <summary>
+    /// Internal method to process reply (implemented by derived classes)
+    /// </summary>
+    protected abstract bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode);
+    
+    /// <summary>
+    /// Adds the COLA2 header to the telegram
+    /// </summary>
+    private byte[] AddTelegramHeader(ReadOnlyMemory<byte> data)
+    {
+        const int headerSize = 18;
+        var totalLength = headerSize + data.Length;
+        var telegram = new byte[totalLength];
+        var span = telegram.AsSpan();
+        
+        // STX (4 bytes): 0x02020202
+        ReadWriteHelper.WriteUint32BigEndian(span, 0, 0x02020202);
+        
+        // Length (4 bytes): 10 + data.Length
+        ReadWriteHelper.WriteUint32BigEndian(span, 4, (uint)(10 + data.Length));
+        
+        // HubCntr (1 byte): 0x00
+        ReadWriteHelper.WriteUint8BigEndian(span, 8, 0x00);
+        
+        // NoC (1 byte): 0x00
+        ReadWriteHelper.WriteUint8BigEndian(span, 9, 0x00);
+        
+        // Session ID (4 bytes)
+        ReadWriteHelper.WriteUint32BigEndian(span, 10, _sessionId);
+        
+        // Request ID (2 bytes)
+        ReadWriteHelper.WriteUint16BigEndian(span, 14, _requestId);
+        
+        // Command Type (1 byte)
+        ReadWriteHelper.WriteUint8BigEndian(span, 16, _commandType);
+        
+        // Command Mode (1 byte)
+        ReadWriteHelper.WriteUint8BigEndian(span, 17, _commandMode);
+        
+        // Copy data
+        data.CopyTo(telegram.AsMemory(headerSize));
+        
+        return telegram;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/CreateSessionCommand.cs

@@ -0,0 +1,50 @@
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to create a new COLA2 session
+/// </summary>
+public sealed class CreateSessionCommand : CommandBase
+{
+    private const byte HeartbeatTimeoutSeconds = 60;
+    private const uint ClientId = 1;
+    
+    public CreateSessionCommand() 
+        : base(0x4F, 0x58) // 'O' and 'X' in ASCII
+    {
+    }
+    
+    public override bool CanBeExecutedWithoutSessionId => true;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        var data = new byte[5];
+        var span = data.AsSpan();
+        
+        // Heartbeat timeout (1 byte)
+        ReadWriteHelper.WriteUint8BigEndian(span, 0, HeartbeatTimeoutSeconds);
+        
+        // Client ID (4 bytes)
+        ReadWriteHelper.WriteUint32BigEndian(span, 1, ClientId);
+        
+        return data;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // Reply should be 'O' (0x4F) and 'A' (0x41) for Acknowledge
+        // Note: Session ID is NOT in reply data, it's in the packet header (offset 10)
+        // In C++ reference, ParseTCPPacket reads session ID from header and sets it to Command
+        // In C# implementation, Cola2Session extracts it from parseResult.SessionId
+        if ((replyCommandType == 0x4F && replyCommandMode == 0x41) ||
+            (replyCommandType == 'O' && replyCommandMode == 'A'))
+        {
+            return true;
+        }
+        
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/FindMeCommand.cs

@@ -0,0 +1,53 @@
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to make the scanner flash/blink to help locate it
+/// </summary>
+public sealed class FindMeCommand : MethodCommand
+{
+    private readonly ushort _blinkTime;
+    
+    public FindMeCommand(ushort blinkTime)
+        : base(14) // Method index for FindMe
+    {
+        _blinkTime = blinkTime;
+    }
+    
+    public ushort BlinkTime => _blinkTime;
+    
+    public override bool CanBeExecutedWithoutSessionId => true;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        // Base method index (2 bytes) + blink time (2 bytes)
+        var data = new byte[2 + 2];
+        var span = data.AsSpan();
+        
+        // Write base method index (from MethodCommand.AddTelegramData)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 0, MethodIndex);
+        
+        // Write blink time (offset 2, 2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 2, _blinkTime);
+        
+        return data;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // According to C++ reference, this inverts the result from base class
+        // Error response: 'E' (0x45) and 'I' (0x49)
+        if ((replyCommandType == 0x45 && replyCommandMode == 0x49) ||
+            (replyCommandType == 'E' && replyCommandMode == 'I'))
+        {
+            return false;
+        }
+        
+        // Success: 'A' (0x41) and 'I' (0x49)
+        return (replyCommandType == 0x41 && replyCommandMode == 0x49) ||
+               (replyCommandType == 'A' && replyCommandMode == 'I');
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/LatestTelegramVariableCommand.cs

@@ -0,0 +1,80 @@
+using Sick.SafetyScanners.DataProcessing;
+using Sick.SafetyScanners.DataStructures;
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to request the latest telegram (measurement data) from the sensor via TCP
+/// Variable index: 179 + channel_index (179 for channel 0, 180 for channel 1, etc.)
+/// </summary>
+public sealed class LatestTelegramVariableCommand : CommandBase
+{
+    private readonly ushort _variableIndex;
+    private readonly ParseData _dataParser;
+    private UdpScanData? _scanData;
+    
+    /// <summary>
+    /// Creates a new LatestTelegramVariableCommand
+    /// </summary>
+    /// <param name="channelIndex">Channel index (0-3), defaults to 0</param>
+    public LatestTelegramVariableCommand(sbyte channelIndex = 0)
+        : base(0x52, 0x49) // 'R' and 'I' in ASCII (Read by Index)
+    {
+        if (channelIndex < 0 || channelIndex > 3)
+        {
+            throw new ArgumentOutOfRangeException(nameof(channelIndex), 
+                "Channel index must be between 0 and 3");
+        }
+        
+        // Variable index: 179 + channel_index
+        _variableIndex = (ushort)(179 + channelIndex);
+        _dataParser = new ParseData();
+    }
+    
+    public ushort VariableIndex => _variableIndex;
+    
+    public override bool CanBeExecutedWithoutSessionId => true;
+    
+    /// <summary>
+    /// Gets the parsed scan data after the command has been executed successfully
+    /// </summary>
+    public UdpScanData? ScanData => _scanData;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        var data = new byte[2];
+        var span = data.AsSpan();
+        
+        // Variable index (2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 0, _variableIndex);
+        
+        return data;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // Reply should be 'R' (0x52) and 'A' (0x41) for Acknowledge
+        if ((replyCommandType == 0x52 && replyCommandMode == 0x41) ||
+            (replyCommandType == 'R' && replyCommandMode == 'A'))
+        {
+            try
+            {
+                // Parse the TCP sequence data
+                // The replyData contains the measurement data payload
+                var packetBuffer = new PacketBuffer(replyData.ToArray(), replyData.Length);
+                _scanData = _dataParser.ParseTcpSequence(packetBuffer);
+                return true;
+            }
+            catch (Exception)
+            {
+                // Parsing failed
+                return false;
+            }
+        }
+        
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/MethodCommand.cs

@@ -0,0 +1,47 @@
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Base command for method calls to the sensor
+/// Thread-safe implementation
+/// </summary>
+public abstract class MethodCommand : CommandBase
+{
+    private readonly ushort _methodIndex;
+    
+    protected MethodCommand(ushort methodIndex)
+        : base(0x4D, 0x49) // 'M' and 'I' in ASCII (Method by Index)
+    {
+        _methodIndex = methodIndex;
+    }
+    
+    public ushort MethodIndex => _methodIndex;
+    
+    public override bool CanBeExecutedWithoutSessionId => false;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        var data = new byte[2];
+        var span = data.AsSpan();
+        
+        // Method index (2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 0, _methodIndex);
+        
+        return data;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // Reply should be 'A' (0x41) and 'I' (0x49) for Acknowledge
+        if ((replyCommandType == 0x41 && replyCommandMode == 0x49) ||
+            (replyCommandType == 'A' && replyCommandMode == 'I'))
+        {
+            return true;
+        }
+        
+        return false;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/Cola2/Commands/VariableCommand.cs

@@ -0,0 +1,46 @@
+using Sick.SafetyScanners.Helpers;
+
+namespace Sick.SafetyScanners.Cola2.Commands;
+
+/// <summary>
+/// Command to read a variable from the sensor by index
+/// </summary>
+public sealed class VariableCommand : CommandBase
+{
+    private readonly ushort _variableIndex;
+    
+    public VariableCommand(ushort variableIndex) 
+        : base(0x52, 0x49) // 'R' and 'I' in ASCII (Read by Index)
+    {
+        _variableIndex = variableIndex;
+    }
+    
+    public ushort VariableIndex => _variableIndex;
+    
+    public override bool CanBeExecutedWithoutSessionId => false;
+    
+    protected override ReadOnlyMemory<byte> AddTelegramData()
+    {
+        var data = new byte[2];
+        var span = data.AsSpan();
+        
+        // Variable index (2 bytes, little endian)
+        ReadWriteHelper.WriteUint16LittleEndian(span, 0, _variableIndex);
+        
+        return data;
+    }
+    
+    protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData, 
+        byte replyCommandType, byte replyCommandMode)
+    {
+        // Reply should be 'R' (0x52) and 'A' (0x41) for Acknowledge
+        if ((replyCommandType == 0x52 && replyCommandMode == 0x41) ||
+            (replyCommandType == 'R' && replyCommandMode == 'A'))
+        {
+            return true;
+        }
+        
+        return false;
+    }
+}
+