Initial commit

srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiClient.cs

@@ -0,0 +1,155 @@
+using System.Text;
+
+namespace RobotNet10.RobotApp.SickScanXdApi;
+
+/// <summary>
+/// High-level C# wrapper for sick_scan_xd API
+/// </summary>
+public class SickScanApiClient : IDisposable
+{
+    private IntPtr _apiHandle;
+    private bool _disposed;
+    private readonly object _lock = new();
+    
+    public bool IsInitialized => _apiHandle != IntPtr.Zero;
+    
+    public SickScanApiClient()
+    {
+        _apiHandle = IntPtr.Zero;
+    }
+    
+    /// <summary>
+    /// Initialize scanner with launchfile-style arguments
+    /// </summary>
+    public async Task<bool> InitializeAsync(string launchfileArgs, CancellationToken ct = default)
+    {
+        await Task.Yield(); // Make async
+        
+        lock (_lock)
+        {
+            if (_apiHandle != IntPtr.Zero)
+                throw new InvalidOperationException("Already initialized");
+            
+            // Create API handle
+            _apiHandle = SickScanApiNative.SickScanApiCreate(0, IntPtr.Zero);
+            if (_apiHandle == IntPtr.Zero)
+                return false;
+            
+            // Split launch args by spaces and convert to argc/argv format
+            // Format: "scanner_type:=sick_tim_7xx hostname:=192.168.254.11 port:=2112 ..."
+            var argsList = new List<string>();
+            argsList.Add("sick_scan_xd_api"); // argv[0] = program name
+            argsList.AddRange(launchfileArgs.Split(' ', StringSplitOptions.RemoveEmptyEntries));
+            
+            int argc = argsList.Count;
+            IntPtr[] argvPtrs = new IntPtr[argc];
+            IntPtr argv = IntPtr.Zero;
+            
+            try
+            {
+                // Allocate argv array
+                argv = System.Runtime.InteropServices.Marshal.AllocHGlobal(argc * IntPtr.Size);
+                
+                // Marshal each string argument
+                for (int i = 0; i < argc; i++)
+                {
+                    argvPtrs[i] = System.Runtime.InteropServices.Marshal.StringToHGlobalAnsi(argsList[i]);
+                }
+                
+                // Copy pointers to argv
+                System.Runtime.InteropServices.Marshal.Copy(argvPtrs, 0, argv, argc);
+                
+                // Initialize with CLI args
+                var result = SickScanApiNative.SickScanApiInitByCli(_apiHandle, argc, argv);
+                
+                if (result != SickScanApiNative.SICK_SCAN_API_SUCCESS)
+                {
+                    SickScanApiNative.SickScanApiRelease(_apiHandle);
+                    _apiHandle = IntPtr.Zero;
+                    return false;
+                }
+                
+                return true;
+            }
+            finally
+            {
+                // Free all allocated memory
+                for (int i = 0; i < argc && i < argvPtrs.Length; i++)
+                {
+                    if (argvPtrs[i] != IntPtr.Zero)
+                        System.Runtime.InteropServices.Marshal.FreeHGlobal(argvPtrs[i]);
+                }
+                if (argv != IntPtr.Zero)
+                    System.Runtime.InteropServices.Marshal.FreeHGlobal(argv);
+            }
+        }
+    }
+    
+    /// <summary>
+    /// Poll for next polar point cloud (blocking with timeout)
+    /// </summary>
+    public SickScanApiNative.SickScanPointCloudMsg? PollPolarPointCloud(double timeoutSec = 1.0)
+    {
+        if (_apiHandle == IntPtr.Zero)
+            throw new InvalidOperationException("Not initialized");
+        
+        var msg = new SickScanApiNative.SickScanPointCloudMsg
+        {
+            header = new SickScanApiNative.SickScanHeader
+            {
+                frame_id = new byte[256]
+            },
+            fields = new SickScanApiNative.SickScanPointFieldArray(),
+            data = new SickScanApiNative.SickScanUint8Array(),
+            topic = new byte[256]
+        };
+        
+        var result = SickScanApiNative.SickScanApiWaitNextPolarPointCloudMsg(
+            _apiHandle, ref msg, timeoutSec);
+        
+        if (result == SickScanApiNative.SICK_SCAN_API_SUCCESS)
+            return msg;
+        
+        if (result == SickScanApiNative.SICK_SCAN_API_TIMEOUT)
+            return null;
+        
+        throw new Exception($"Failed to poll point cloud: error code {result}");
+    }
+    
+    /// <summary>
+    /// Free point cloud message memory
+    /// </summary>
+    public void FreePointCloudMsg(ref SickScanApiNative.SickScanPointCloudMsg msg)
+    {
+        if (_apiHandle == IntPtr.Zero)
+            return;
+        
+        SickScanApiNative.SickScanApiFreePointCloudMsg(_apiHandle, ref msg);
+    }
+    
+    /// <summary>
+    /// Close scanner connection
+    /// </summary>
+    public void Close()
+    {
+        lock (_lock)
+        {
+            if (_apiHandle == IntPtr.Zero)
+                return;
+            
+            SickScanApiNative.SickScanApiClose(_apiHandle);
+            SickScanApiNative.SickScanApiRelease(_apiHandle);
+            _apiHandle = IntPtr.Zero;
+        }
+    }
+    
+    public void Dispose()
+    {
+        if (_disposed)
+            return;
+        
+        Close();
+        _disposed = true;
+        GC.SuppressFinalize(this);
+    }
+}