Initial commit

2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiNative.cs
+++ b/srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiNative.cs
@@ -0,0 +1,124 @@
+using System.Runtime.InteropServices;
+
+namespace RobotNet10.RobotApp.SickScanXdApi;
+
+/// <summary>
+/// P/Invoke declarations for sick_scan_xd C API
+/// </summary>
+public static class SickScanApiNative
+{
+    private const string LibraryName = "sick_scan_xd_shared_lib"; // libsick_scan_xd_shared_lib.so
+    
+    // Structs matching C API (exact field names with snake_case)
+    
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SickScanHeader
+    {
+        public uint seq;
+        public uint timestamp_sec;
+        public uint timestamp_nsec;
+        
+        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
+        public byte[] frame_id;
+    }
+    
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SickScanUint8Array
+    {
+        public ulong capacity;
+        public ulong size;
+        public IntPtr buffer;  // uint8_t*
+    }
+    
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SickScanPointFieldMsg
+    {
+        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
+        public byte[] name;
+        public uint offset;
+        public byte datatype;
+        public uint count;
+    }
+    
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SickScanPointFieldArray
+    {
+        public ulong capacity;
+        public ulong size;
+        public IntPtr buffer;  // SickScanPointFieldMsg*
+    }
+    
+    [StructLayout(LayoutKind.Sequential)]
+    public struct SickScanPointCloudMsg
+    {
+        public SickScanHeader header;
+        public uint height;
+        public uint width;
+        public SickScanPointFieldArray fields;
+        public byte is_bigendian;
+        public uint point_step;
+        public uint row_step;
+        public SickScanUint8Array data;
+        public byte is_dense;
+        public int num_echos;
+        public int segment_idx;
+        
+        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 256)]
+        public byte[] topic;
+    }
+    
+    // API Functions
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern IntPtr SickScanApiCreate(int argc, IntPtr argv);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiRelease(IntPtr apiHandle);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiInitByCli(
+        IntPtr apiHandle,
+        int argc,
+        IntPtr argv);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiInitByLaunchfile(
+        IntPtr apiHandle,
+        IntPtr launchfileArgs);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiClose(IntPtr apiHandle);
+    
+    // Callback registration
+    public delegate void PointCloudMsgCallback(IntPtr apiHandle, ref SickScanPointCloudMsg msg);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiRegisterPolarPointCloudMsg(
+        IntPtr apiHandle,
+        PointCloudMsgCallback callback);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiDeregisterPolarPointCloudMsg(
+        IntPtr apiHandle,
+        PointCloudMsgCallback callback);
+    
+    // Polling function
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiWaitNextPolarPointCloudMsg(
+        IntPtr apiHandle,
+        ref SickScanPointCloudMsg msg,
+        double timeoutSec);
+    
+    [DllImport(LibraryName, CallingConvention = CallingConvention.Cdecl)]
+    public static extern int SickScanApiFreePointCloudMsg(
+        IntPtr apiHandle,
+        ref SickScanPointCloudMsg msg);
+    
+    // Error codes
+    public const int SICK_SCAN_API_SUCCESS = 0;
+    public const int SICK_SCAN_API_ERROR = 1;
+    public const int SICK_SCAN_API_NOT_LOADED = 2;
+    public const int SICK_SCAN_API_NOT_INITIALIZED = 3;
+    public const int SICK_SCAN_API_NOT_IMPLEMENTED = 4;
+    public const int SICK_SCAN_API_TIMEOUT = 5;
+}