Initial commit

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;
+    }
+}
+