Initial commit

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

@@ -0,0 +1,119 @@
+using Sick.Tim781s.Colab.Commands;
+using Sick.Tim781s.Colab.Helpers;
+using Sick.Tim781s.Colab.Interfaces;
+
+namespace Sick.Tim781s.Colab;
+
+/// <summary>
+/// ColaB session manager for handling send, receive and process telegrams
+/// </summary>
+public sealed class ColaBSession : IDisposable
+{
+    private readonly ITcpClient _tcpClient;
+    private readonly object _lock = new();
+    private bool _disposed;
+
+    public bool IsOpen
+    {
+        get
+        {
+            lock (_lock)
+            {
+                return _tcpClient.IsConnected;
+            }
+        }
+    }
+
+    public ColaBSession(ITcpClient tcpClient)
+    {
+        _tcpClient = tcpClient ?? throw new ArgumentNullException(nameof(tcpClient));
+    }
+
+    /// <summary>
+    /// Opens the session (connects to scanner)
+    /// </summary>
+    public void Open(int timeoutMs = 5000)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(ColaBSession));
+
+        if (IsOpen)
+            return;
+
+        _tcpClient.Connect(timeoutMs);
+    }
+
+    /// <summary>
+    /// Closes the session (disconnects from scanner)
+    /// </summary>
+    public void Close()
+    {
+        if (_disposed)
+            return;
+
+        _tcpClient.Disconnect();
+    }
+
+    /// <summary>
+    /// Executes a command and waits for reply
+    /// </summary>
+    public void ExecuteCommand(IColaBCommand command, int timeoutMs = 5000)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(nameof(ColaBSession));
+
+        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 _))
+        {
+            // Process reply
+            command.ProcessReply(replyData.ToArray());
+        }
+        else
+        {
+            throw new InvalidOperationException($"Failed to parse reply frame. Raw reply: {BitConverter.ToString(replyFrame.Take(64).ToArray())}");
+        }
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        Close();
+        _disposed = true;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.Tim781s.Colab/Commands/CommandBase.cs

@@ -0,0 +1,37 @@
+using System.Text;
+using Sick.Tim781s.Colab.Helpers;
+
+namespace Sick.Tim781s.Colab.Commands;
+
+/// <summary>
+/// Base class for ColaB commands
+/// </summary>
+public abstract class CommandBase : IColaBCommand
+{
+    private readonly string _commandString;
+    private bool _wasSuccessful;
+
+    protected CommandBase(string commandString)
+    {
+        _commandString = commandString ?? throw new ArgumentNullException(nameof(commandString));
+    }
+
+    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.Tim781s.Colab/Commands/MethodCommand.cs

@@ -0,0 +1,55 @@
+using System.Text;
+
+namespace Sick.Tim781s.Colab.Commands;
+
+/// <summary>
+/// Command to call a method on the scanner (sMN - Method by Name)
+/// </summary>
+public sealed class MethodCommand : CommandBase
+{
+    public MethodCommand(string methodName, params string[] parameters) 
+        : base($"sMN {methodName} {string.Join(" ", parameters)}")
+    {
+        MethodName = methodName;
+        Parameters = parameters;
+    }
+
+    public string MethodName { get; }
+    public string[] Parameters { get; }
+
+    public string? ReplyValue { get; private set; }
+
+    public override bool ProcessReply(ReadOnlyMemory<byte> replyData)
+    {
+        // Parse reply: "sAN <methodName> <result>" or "sMA <methodName> <result>"
+        var replyString = Encoding.ASCII.GetString(replyData.Span);
+        
+        // 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.Tim781s.Colab/Commands/ReadCommand.cs

@@ -0,0 +1,77 @@
+using System.Text;
+using Sick.Tim781s.Colab.Helpers;
+
+namespace Sick.Tim781s.Colab.Commands;
+
+/// <summary>
+/// Command to read a value from the scanner (sRN - Read by Name)
+/// </summary>
+public sealed class ReadCommand : CommandBase
+{
+    public ReadCommand(string variableName) 
+        : base($"sRN {variableName}")
+    {
+        VariableName = variableName;
+    }
+
+    public string VariableName { get; }
+
+    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.Slice(0, 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.Tim781s.Colab/Commands/WriteCommand.cs

@@ -0,0 +1,45 @@
+using System.Text;
+
+namespace Sick.Tim781s.Colab.Commands;
+
+/// <summary>
+/// Command to write a value to the scanner (sWN - Write by Name)
+/// </summary>
+public sealed class WriteCommand : CommandBase
+{
+    public WriteCommand(string variableName, string value) 
+        : base($"sWN {variableName} {value}")
+    {
+        VariableName = variableName;
+        Value = value;
+    }
+
+    public string VariableName { get; }
+    public string Value { get; }
+
+    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))
+        {
+            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.Tim781s.Colab/Communication/TcpClient.cs

@@ -0,0 +1,235 @@
+using System.Net;
+using System.Net.Sockets;
+using Sick.Tim781s.Colab.Interfaces;
+
+namespace Sick.Tim781s.Colab.Communication;
+
+/// <summary>
+/// TCP client implementation for ColaB communication
+/// </summary>
+public sealed class TcpClient : ITcpClient
+{
+    private readonly string _serverIp;
+    private readonly ushort _serverPort;
+    private System.Net.Sockets.TcpClient? _tcpClient;
+    private NetworkStream? _stream;
+    private readonly object _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 TcpClient(string serverIp, ushort serverPort)
+    {
+        _serverIp = serverIp ?? throw new ArgumentNullException(nameof(serverIp));
+        _serverPort = serverPort;
+    }
+
+    public void Connect(int timeoutMs = 5000)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(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 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)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(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)
+    {
+        if (_disposed)
+            throw new ObjectDisposedException(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");
+            }
+
+            // 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
+            var remainingBytes = (int)payloadLength + 1; // payload + checksum
+            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;
+    }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.Tim781s.Colab/Helpers/ColaBHelper.cs

@@ -0,0 +1,211 @@
+using System.Text;
+
+namespace Sick.Tim781s.Colab.Helpers;
+
+/// <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.Tim781s.Colab/Interfaces/IColaBCommand.cs

@@ -0,0 +1,25 @@
+namespace Sick.Tim781s.Colab.Commands;
+
+/// <summary>
+/// Interface for ColaB protocol commands
+/// </summary>
+public interface IColaBCommand
+{
+    /// <summary>
+    /// Gets the command data (SOPAS command string encoded as bytes)
+    /// </summary>
+    ReadOnlyMemory<byte> GetCommandData();
+
+    /// <summary>
+    /// Processes the reply from the scanner
+    /// </summary>
+    /// <param name="replyData">The reply data from the scanner</param>
+    /// <returns>True if the command was successful</returns>
+    bool ProcessReply(ReadOnlyMemory<byte> replyData);
+
+    /// <summary>
+    /// Indicates if the command was successfully executed
+    /// </summary>
+    bool WasSuccessful { get; }
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.Tim781s.Colab/Interfaces/ITcpClient.cs

@@ -0,0 +1,47 @@
+namespace Sick.Tim781s.Colab.Interfaces;
+
+/// <summary>
+/// Interface for TCP client communication
+/// </summary>
+public interface ITcpClient
+{
+    /// <summary>
+    /// Gets the server IP address
+    /// </summary>
+    string ServerIp { get; }
+
+    /// <summary>
+    /// Gets the server port
+    /// </summary>
+    ushort ServerPort { get; }
+
+    /// <summary>
+    /// Gets whether the client is connected
+    /// </summary>
+    bool IsConnected { get; }
+
+    /// <summary>
+    /// Connects to the server
+    /// </summary>
+    /// <param name="timeoutMs">Connection timeout in milliseconds</param>
+    void Connect(int timeoutMs = 5000);
+
+    /// <summary>
+    /// Disconnects from the server
+    /// </summary>
+    void Disconnect();
+
+    /// <summary>
+    /// Sends data to the server
+    /// </summary>
+    /// <param name="data">Data to send</param>
+    void Send(ReadOnlyMemory<byte> data);
+
+    /// <summary>
+    /// Receives data from the server
+    /// </summary>
+    /// <param name="timeoutMs">Receive timeout in milliseconds</param>
+    /// <returns>Received data</returns>
+    byte[] Receive(int timeoutMs = 5000);
+}
+

srcs/RobotNet10/RobotApp/Communication/Sick.Tim781s.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.Tim781s.Colab/Sick.Tim781s.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.Tim781s.Colab/Tim781sScanner.cs

@@ -0,0 +1,133 @@
+using Sick.Tim781s.Colab;
+using Sick.Tim781s.Colab.Commands;
+using Sick.Tim781s.Colab.Communication;
+
+namespace Sick.Tim781s.Colab;
+
+/// <summary>
+/// Main class for communicating with SICK TiM781s scanner using ColaB protocol
+/// </summary>
+public sealed class Tim781sScanner : IDisposable
+{
+    private readonly ColaBSession _session;
+    private readonly TcpClient _tcpClient;
+    private bool _disposed;
+
+    public Tim781sScanner(string ipAddress, ushort port = 2112)
+    {
+        _tcpClient = new TcpClient(ipAddress, port);
+        _session = new ColaBSession(_tcpClient);
+    }
+
+    /// <summary>
+    /// Connects to the scanner
+    /// </summary>
+    public void Connect(int timeoutMs = 5000)
+    {
+        _session.Open(timeoutMs);
+    }
+
+    /// <summary>
+    /// Disconnects from the scanner
+    /// </summary>
+    public void Disconnect()
+    {
+        _session.Close();
+    }
+
+    /// <summary>
+    /// Gets whether the scanner is connected
+    /// </summary>
+    public bool IsConnected => _session.IsOpen;
+
+    /// <summary>
+    /// Reads device identification
+    /// </summary>
+    public string? ReadDeviceIdent(int timeoutMs = 5000)
+    {
+        var command = new ReadCommand("DeviceIdent");
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.ReplyValue;
+    }
+
+    /// <summary>
+    /// Reads serial number
+    /// </summary>
+    public string? ReadSerialNumber(int timeoutMs = 5000)
+    {
+        var command = new ReadCommand("SerialNumber");
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.ReplyValue;
+    }
+
+    /// <summary>
+    /// Reads firmware version
+    /// </summary>
+    public string? ReadFirmwareVersion(int timeoutMs = 5000)
+    {
+        var command = new ReadCommand("FirmwareVersion");
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.ReplyValue;
+    }
+
+    /// <summary>
+    /// Starts measurement
+    /// </summary>
+    public bool StartMeasurement(int timeoutMs = 5000)
+    {
+        var command = new MethodCommand("mNEVAChangeState", "1"); // 1 = measurement mode
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.WasSuccessful;
+    }
+
+    /// <summary>
+    /// Stops measurement
+    /// </summary>
+    public bool StopMeasurement(int timeoutMs = 5000)
+    {
+        var command = new MethodCommand("mNEVAChangeState", "0"); // 0 = standby mode
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.WasSuccessful;
+    }
+
+    /// <summary>
+    /// Executes a custom read command
+    /// </summary>
+    public string? ExecuteRead(string variableName, int timeoutMs = 5000)
+    {
+        var command = new ReadCommand(variableName);
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.ReplyValue;
+    }
+
+    /// <summary>
+    /// Executes a custom write command
+    /// </summary>
+    public bool ExecuteWrite(string variableName, string value, int timeoutMs = 5000)
+    {
+        var command = new WriteCommand(variableName, value);
+        _session.ExecuteCommand(command, timeoutMs);
+        return command.WasSuccessful;
+    }
+
+    /// <summary>
+    /// Executes a custom method command
+    /// </summary>
+    public string? ExecuteMethod(string methodName, params string[] parameters)
+    {
+        var command = new MethodCommand(methodName, parameters);
+        _session.ExecuteCommand(command);
+        return command.ReplyValue;
+    }
+
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+
+        _session?.Dispose();
+        _tcpClient?.Dispose();
+        _disposed = true;
+    }
+}
+