feat: load pinned OSQP native library

2026-08-03 23:48:41 +08:00
parent 2abb465d98
commit 39c1708c48
5 changed files with 608 additions and 2 deletions
@@ -0,0 +1,110 @@
+using System;
+using System.IO;
+using System.Reflection;
+using System.Runtime.InteropServices;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class OsqpNativeLoadResult
+{
+    internal OsqpNativeLoadResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic)
+    {
+        Status = status;
+        Version = version ?? string.Empty;
+        ModuleHandle = moduleHandle;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    public QpSolveStatus Status { get; }
+
+    public string Version { get; }
+
+    public IntPtr ModuleHandle { get; }
+
+    public string Diagnostic { get; }
+}
+
+public static class OsqpNativeLoader
+{
+    private static readonly object Sync = new object();
+    private static OsqpNativeLoadResult cachedResult;
+    private static OsqpNativeApi loadedApi;
+
+    public static OsqpNativeLoadResult Load()
+    {
+        lock (Sync)
+        {
+            if (cachedResult != null)
+                return cachedResult;
+
+            if (IntPtr.Size != 8)
+                return CacheFailure(QpSolveStatus.SolverUnavailable, "OSQP v1.0.0 requires a 64-bit process.");
+
+            try
+            {
+                OsqpNativeStructures.ValidatePinnedLayout();
+                string nativePath = ResolveNativePath();
+                if (!File.Exists(nativePath))
+                    return CacheFailure(QpSolveStatus.SolverUnavailable, "OSQP native library was not found at " + nativePath + ".");
+
+                IntPtr moduleHandle = OsqpNativeMethods.LoadLibrary(nativePath);
+                if (moduleHandle == IntPtr.Zero)
+                    return CacheFailure(QpSolveStatus.SolverUnavailable,
+                        "OSQP native library could not be loaded from " + nativePath + ": " + OsqpNativeMethods.LastErrorMessage());
+
+                try
+                {
+                    OsqpNativeApi api = OsqpNativeMethods.ResolveApi(moduleHandle);
+                    string version = Marshal.PtrToStringAnsi(api.Version()) ?? string.Empty;
+                    if (!string.Equals(version, "1.0.0", StringComparison.Ordinal))
+                    {
+                        OsqpNativeMethods.FreeModule(moduleHandle);
+                        return CacheFailure(QpSolveStatus.SolverUnavailable,
+                            "OSQP native library version " + version + " does not match required version 1.0.0.");
+                    }
+
+                    loadedApi = api;
+                    cachedResult = new OsqpNativeLoadResult(QpSolveStatus.Solved, version, moduleHandle, string.Empty);
+                    return cachedResult;
+                }
+                catch (Exception exception)
+                {
+                    OsqpNativeMethods.FreeModule(moduleHandle);
+                    return CacheFailure(QpSolveStatus.SolverUnavailable,
+                        "OSQP native library exports could not be initialized: " + exception.Message);
+                }
+            }
+            catch (Exception exception)
+            {
+                return CacheFailure(QpSolveStatus.NativeError, "OSQP native loader failed: " + exception.Message);
+            }
+        }
+    }
+
+    internal static bool TryGetLoadedApi(out OsqpNativeApi api, out OsqpNativeLoadResult result)
+    {
+        result = Load();
+        api = loadedApi;
+        return result.Status == QpSolveStatus.Solved && api != null;
+    }
+
+    private static OsqpNativeLoadResult CacheFailure(QpSolveStatus status, string diagnostic)
+    {
+        cachedResult = new OsqpNativeLoadResult(status, string.Empty, IntPtr.Zero, diagnostic);
+        return cachedResult;
+    }
+
+    private static string ResolveNativePath()
+    {
+        Assembly assembly = typeof(OsqpNativeLoader).Assembly;
+        string assemblyLocation = assembly.Location;
+        if (string.IsNullOrEmpty(assemblyLocation) || !Path.IsPathRooted(assemblyLocation))
+            throw new InvalidOperationException("The EM Planner assembly location must be an absolute file path.");
+
+        string pluginDirectory = Path.GetDirectoryName(assemblyLocation);
+        if (string.IsNullOrEmpty(pluginDirectory))
+            throw new InvalidOperationException("The EM Planner assembly location has no plugin directory.");
+
+        return Path.GetFullPath(Path.Combine(pluginDirectory, "osqp.dll"));
+    }
+}