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"));
+    }
+}
@@ -0,0 +1,111 @@
+using System;
+using System.ComponentModel;
+using System.Runtime.InteropServices;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class OsqpNativeMethods
+{
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate IntPtr VersionDelegate();
+
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate void SetDefaultSettingsDelegate(IntPtr settings);
+
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate int SetupDelegate(
+        out IntPtr solver,
+        IntPtr upperTriangularP,
+        IntPtr linearCost,
+        IntPtr constraints,
+        IntPtr lowerBounds,
+        IntPtr upperBounds,
+        int constraintCount,
+        int variableCount,
+        IntPtr settings);
+
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate int WarmStartDelegate(IntPtr solver, IntPtr primal, IntPtr dual);
+
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate int SolveDelegate(IntPtr solver);
+
+    [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+    internal delegate int CleanupDelegate(IntPtr solver);
+
+    public static IntPtr LoadLibrary(string absolutePath)
+    {
+        return LoadLibraryW(absolutePath);
+    }
+
+    public static bool FreeModule(IntPtr moduleHandle)
+    {
+        return FreeLibrary(moduleHandle);
+    }
+
+    public static string LastErrorMessage()
+    {
+        int error = Marshal.GetLastWin32Error();
+        return new Win32Exception(error).Message;
+    }
+
+    public static OsqpNativeApi ResolveApi(IntPtr moduleHandle)
+    {
+        return new OsqpNativeApi(
+            Resolve<VersionDelegate>(moduleHandle, "osqp_version"),
+            Resolve<SetDefaultSettingsDelegate>(moduleHandle, "osqp_set_default_settings"),
+            Resolve<SetupDelegate>(moduleHandle, "osqp_setup"),
+            Resolve<WarmStartDelegate>(moduleHandle, "osqp_warm_start"),
+            Resolve<SolveDelegate>(moduleHandle, "osqp_solve"),
+            Resolve<CleanupDelegate>(moduleHandle, "osqp_cleanup"));
+    }
+
+    private static T Resolve<T>(IntPtr moduleHandle, string exportName) where T : Delegate
+    {
+        IntPtr procedure = GetProcAddress(moduleHandle, exportName);
+        if (procedure == IntPtr.Zero)
+            throw new InvalidOperationException("Missing OSQP export " + exportName + ".");
+        return Marshal.GetDelegateForFunctionPointer<T>(procedure);
+    }
+
+    [DllImport("kernel32", CharSet = CharSet.Unicode, ExactSpelling = true, SetLastError = true)]
+    private static extern IntPtr LoadLibraryW(string fileName);
+
+    [DllImport("kernel32", ExactSpelling = true, SetLastError = true)]
+    [return: MarshalAs(UnmanagedType.Bool)]
+    private static extern bool FreeLibrary(IntPtr moduleHandle);
+
+    [DllImport("kernel32", CharSet = CharSet.Ansi, ExactSpelling = true, SetLastError = true)]
+    private static extern IntPtr GetProcAddress(IntPtr moduleHandle, string procedureName);
+}
+
+internal sealed class OsqpNativeApi
+{
+    public OsqpNativeApi(
+        OsqpNativeMethods.VersionDelegate version,
+        OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings,
+        OsqpNativeMethods.SetupDelegate setup,
+        OsqpNativeMethods.WarmStartDelegate warmStart,
+        OsqpNativeMethods.SolveDelegate solve,
+        OsqpNativeMethods.CleanupDelegate cleanup)
+    {
+        Version = version;
+        SetDefaultSettings = setDefaultSettings;
+        Setup = setup;
+        WarmStart = warmStart;
+        Solve = solve;
+        Cleanup = cleanup;
+    }
+
+    public OsqpNativeMethods.VersionDelegate Version { get; }
+
+    public OsqpNativeMethods.SetDefaultSettingsDelegate SetDefaultSettings { get; }
+
+    public OsqpNativeMethods.SetupDelegate Setup { get; }
+
+    public OsqpNativeMethods.WarmStartDelegate WarmStart { get; }
+
+    public OsqpNativeMethods.SolveDelegate Solve { get; }
+
+    public OsqpNativeMethods.CleanupDelegate Cleanup { get; }
+}
@@ -0,0 +1,205 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+[StructLayout(LayoutKind.Sequential, Pack = 8)]
+internal struct OsqpCscMatrix
+{
+    public int RowCount;
+    public int ColumnCount;
+    public IntPtr ColumnPointers;
+    public IntPtr RowIndices;
+    public IntPtr Values;
+    public int MaximumNonZeroCount;
+    public int NonZeroCount;
+    public int OwnsData;
+}
+
+[StructLayout(LayoutKind.Sequential, Pack = 8)]
+internal struct OsqpSettings
+{
+    public int Device;
+    public int LinearSystemSolver;
+    public int AllocateSolution;
+    public int Verbose;
+    public int ProfilerLevel;
+    public int WarmStarting;
+    public int Scaling;
+    public int Polishing;
+    public double Rho;
+    public int RhoIsVector;
+    public double Sigma;
+    public double Alpha;
+    public int ConjugateGradientMaximumIterations;
+    public int ConjugateGradientToleranceReduction;
+    public double ConjugateGradientToleranceFraction;
+    public int ConjugateGradientPreconditioner;
+    public int AdaptiveRho;
+    public int AdaptiveRhoInterval;
+    public double AdaptiveRhoFraction;
+    public double AdaptiveRhoTolerance;
+    public int MaximumIterations;
+    public double AbsoluteTolerance;
+    public double RelativeTolerance;
+    public double PrimalInfeasibilityTolerance;
+    public double DualInfeasibilityTolerance;
+    public int ScaledTermination;
+    public int CheckTermination;
+    public int CheckDualityGap;
+    public double TimeLimit;
+    public double PolishingDelta;
+    public int PolishingRefinementIterations;
+}
+
+[StructLayout(LayoutKind.Sequential, Pack = 8)]
+internal struct OsqpInfo
+{
+    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 32, ArraySubType = UnmanagedType.I1)]
+    public byte[] Status;
+    public int StatusValue;
+    public int PolishingStatus;
+    public double ObjectiveValue;
+    public double DualObjectiveValue;
+    public double PrimalResidual;
+    public double DualResidual;
+    public double DualityGap;
+    public int Iterations;
+    public int RhoUpdates;
+    public double RhoEstimate;
+    public double SetupTime;
+    public double SolveTime;
+    public double UpdateTime;
+    public double PolishingTime;
+    public double RunTime;
+    public double PrimalDualIntegral;
+    public double RelativeKktError;
+}
+
+[StructLayout(LayoutKind.Sequential, Pack = 8)]
+internal struct OsqpSolution
+{
+    public IntPtr Primal;
+    public IntPtr Dual;
+    public IntPtr PrimalInfeasibilityCertificate;
+    public IntPtr DualInfeasibilityCertificate;
+}
+
+[StructLayout(LayoutKind.Sequential, Pack = 8)]
+internal struct OsqpSolverPrefix
+{
+    public IntPtr Settings;
+    public IntPtr Solution;
+    public IntPtr Info;
+    public IntPtr Workspace;
+}
+
+internal static class OsqpNativeStructures
+{
+    public static void ValidatePinnedLayout()
+    {
+        AssertSize(typeof(OsqpCscMatrix), 48);
+        AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.ColumnPointers), 8);
+        AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.Values), 24);
+        AssertOffset(typeof(OsqpCscMatrix), nameof(OsqpCscMatrix.MaximumNonZeroCount), 32);
+
+        AssertSize(typeof(OsqpSettings), 192);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.Rho), 32);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.Sigma), 48);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.ConjugateGradientToleranceFraction), 72);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.AdaptiveRhoFraction), 96);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.AbsoluteTolerance), 120);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.TimeLimit), 168);
+        AssertOffset(typeof(OsqpSettings), nameof(OsqpSettings.PolishingRefinementIterations), 184);
+
+        AssertSize(typeof(OsqpInfo), 152);
+        AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.StatusValue), 32);
+        AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.ObjectiveValue), 40);
+        AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.Iterations), 80);
+        AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.SetupTime), 96);
+        AssertOffset(typeof(OsqpInfo), nameof(OsqpInfo.RelativeKktError), 144);
+
+        AssertSize(typeof(OsqpSolution), 32);
+        AssertSize(typeof(OsqpSolverPrefix), 32);
+        AssertOffset(typeof(OsqpSolverPrefix), nameof(OsqpSolverPrefix.Workspace), 24);
+    }
+
+    public static OsqpNativeSetupMemory AllocateSetupMemory(OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings)
+    {
+        if (setDefaultSettings == null)
+            throw new ArgumentNullException(nameof(setDefaultSettings));
+
+        return new OsqpNativeSetupMemory(setDefaultSettings);
+    }
+
+    private static void AssertSize(Type type, int expectedSize)
+    {
+        int actualSize = Marshal.SizeOf(type);
+        if (actualSize != expectedSize)
+            throw new InvalidOperationException(type.Name + " has native size " + actualSize + " instead of " + expectedSize + ".");
+    }
+
+    private static void AssertOffset(Type type, string fieldName, int expectedOffset)
+    {
+        int actualOffset = checked((int)Marshal.OffsetOf(type, fieldName).ToInt64());
+        if (actualOffset != expectedOffset)
+            throw new InvalidOperationException(type.Name + "." + fieldName + " has native offset " + actualOffset + " instead of " + expectedOffset + ".");
+    }
+}
+
+internal sealed class OsqpNativeSetupMemory : IDisposable
+{
+    private IntPtr upperTriangularP;
+    private IntPtr constraints;
+    private IntPtr settings;
+
+    public OsqpNativeSetupMemory(OsqpNativeMethods.SetDefaultSettingsDelegate setDefaultSettings)
+    {
+        try
+        {
+            upperTriangularP = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpCscMatrix)));
+            constraints = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpCscMatrix)));
+            settings = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(OsqpSettings)));
+            Zero(upperTriangularP, Marshal.SizeOf(typeof(OsqpCscMatrix)));
+            Zero(constraints, Marshal.SizeOf(typeof(OsqpCscMatrix)));
+            Zero(settings, Marshal.SizeOf(typeof(OsqpSettings)));
+            setDefaultSettings(settings);
+        }
+        catch
+        {
+            Dispose();
+            throw;
+        }
+    }
+
+    public IntPtr UpperTriangularP { get { return upperTriangularP; } }
+
+    public IntPtr Constraints { get { return constraints; } }
+
+    public IntPtr Settings { get { return settings; } }
+
+    public void Dispose()
+    {
+        if (settings != IntPtr.Zero)
+        {
+            Marshal.FreeHGlobal(settings);
+            settings = IntPtr.Zero;
+        }
+        if (constraints != IntPtr.Zero)
+        {
+            Marshal.FreeHGlobal(constraints);
+            constraints = IntPtr.Zero;
+        }
+        if (upperTriangularP != IntPtr.Zero)
+        {
+            Marshal.FreeHGlobal(upperTriangularP);
+            upperTriangularP = IntPtr.Zero;
+        }
+    }
+
+    private static void Zero(IntPtr memory, int byteCount)
+    {
+        for (int index = 0; index < byteCount; index++)
+            Marshal.WriteByte(memory, index, 0);
+    }
+}
@@ -0,0 +1,170 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Threading.Tasks;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class OsqpChecks
+{
+    public static void Run()
+    {
+        var pluginDirectories = new List<string>();
+        try
+        {
+            string missingNativeDirectory = CreatePluginDirectory();
+            pluginDirectories.Add(missingNativeDirectory);
+            ProbeResult missingNative = RunProbe(missingNativeDirectory);
+            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, missingNative.Status, "missing native status");
+            EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(missingNative.Diagnostic), "missing native diagnostic");
+
+            string corruptNativeDirectory = CreatePluginDirectory();
+            pluginDirectories.Add(corruptNativeDirectory);
+            File.WriteAllText(Path.Combine(corruptNativeDirectory, "osqp.dll"), "not a native library");
+            ProbeResult corruptNative = RunProbe(corruptNativeDirectory);
+            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, corruptNative.Status, "corrupt native status");
+            EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(corruptNative.Diagnostic), "corrupt native diagnostic");
+            EMPlannerVerificationHost.Verification.True(corruptNative.RawOutput.IndexOf("BadImageFormatException", StringComparison.Ordinal) < 0,
+                "corrupt native does not escape BadImageFormatException");
+
+            string realNativeDirectory = CreatePluginDirectory();
+            pluginDirectories.Add(realNativeDirectory);
+            string realNativePath = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
+            EMPlannerVerificationHost.Verification.True(File.Exists(realNativePath), "pinned native package exists");
+            File.Copy(realNativePath, Path.Combine(realNativeDirectory, "osqp.dll"), false);
+            ProbeResult realNative = RunProbe(realNativeDirectory);
+            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, realNative.Status, "real native status");
+            EMPlannerVerificationHost.Verification.Equal("1.0.0", realNative.Version, "real native version");
+            EMPlannerVerificationHost.Verification.True(realNative.ModuleHandle != IntPtr.Zero, "real native module handle");
+            EMPlannerVerificationHost.Verification.Equal(true, realNative.ConcurrentHandleStable, "concurrent native module handle");
+        }
+        finally
+        {
+            for (int index = 0; index < pluginDirectories.Count; index++)
+            {
+                if (Directory.Exists(pluginDirectories[index]))
+                    Directory.Delete(pluginDirectories[index], true);
+            }
+        }
+    }
+
+    public static void RunProbe()
+    {
+        var results = new OsqpNativeLoadResult[16];
+        Parallel.For(0, results.Length, index => results[index] = OsqpNativeLoader.Load());
+
+        OsqpNativeLoadResult first = results[0];
+        bool sameHandle = true;
+        for (int index = 1; index < results.Length; index++)
+        {
+            sameHandle = sameHandle && results[index].Status == first.Status && results[index].ModuleHandle == first.ModuleHandle;
+        }
+
+        Console.WriteLine("STATUS=" + first.Status);
+        Console.WriteLine("VERSION=" + first.Version);
+        Console.WriteLine("HANDLE=" + first.ModuleHandle.ToInt64());
+        Console.WriteLine("DIAGNOSTIC=" + first.Diagnostic.Replace('\r', ' ').Replace('\n', ' '));
+        Console.WriteLine("CONCURRENT_HANDLE_STABLE=" + sameHandle);
+    }
+
+    private static string CreatePluginDirectory()
+    {
+        string sourceDirectory = AppContext.BaseDirectory;
+        string destinationDirectory = Path.Combine(Path.GetTempPath(), "em-planner-osqp-loader-" + Guid.NewGuid().ToString("N"));
+        Directory.CreateDirectory(destinationDirectory);
+
+        string[] hostFiles = Directory.GetFiles(sourceDirectory);
+        for (int index = 0; index < hostFiles.Length; index++)
+        {
+            string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
+            File.Copy(hostFiles[index], destinationPath, false);
+        }
+
+        EMPlannerVerificationHost.Verification.True(File.Exists(Path.Combine(destinationDirectory, "ClumsyPilot.dll")), "copied plugin ClumsyPilot.dll");
+        return destinationDirectory;
+    }
+
+    private static ProbeResult RunProbe(string pluginDirectory)
+    {
+        var startInfo = new ProcessStartInfo
+        {
+            FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
+            Arguments = "osqp-probe",
+            WorkingDirectory = pluginDirectory,
+            UseShellExecute = false,
+            CreateNoWindow = true,
+            RedirectStandardOutput = true,
+            RedirectStandardError = true,
+        };
+
+        using (var process = new Process { StartInfo = startInfo })
+        {
+            process.Start();
+            string standardOutput = process.StandardOutput.ReadToEnd();
+            string standardError = process.StandardError.ReadToEnd();
+            process.WaitForExit();
+            if (process.ExitCode != 0)
+                throw new InvalidOperationException("OSQP loader probe exited " + process.ExitCode + ": " + standardError + standardOutput);
+            return ProbeResult.Parse(standardOutput + standardError);
+        }
+    }
+
+    private sealed class ProbeResult
+    {
+        private ProbeResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic, bool concurrentHandleStable, string rawOutput)
+        {
+            Status = status;
+            Version = version;
+            ModuleHandle = moduleHandle;
+            Diagnostic = diagnostic;
+            ConcurrentHandleStable = concurrentHandleStable;
+            RawOutput = rawOutput;
+        }
+
+        public QpSolveStatus Status { get; }
+
+        public string Version { get; }
+
+        public IntPtr ModuleHandle { get; }
+
+        public string Diagnostic { get; }
+
+        public bool ConcurrentHandleStable { get; }
+
+        public string RawOutput { get; }
+
+        public static ProbeResult Parse(string output)
+        {
+            string statusText = ReadValue(output, "STATUS=");
+            string version = ReadValue(output, "VERSION=");
+            string handleText = ReadValue(output, "HANDLE=");
+            string diagnostic = ReadValue(output, "DIAGNOSTIC=");
+            string stableText = ReadValue(output, "CONCURRENT_HANDLE_STABLE=");
+
+            QpSolveStatus status;
+            if (!Enum.TryParse(statusText, out status))
+                throw new InvalidOperationException("Loader probe reported an invalid status: " + statusText);
+            long handleValue;
+            if (!long.TryParse(handleText, out handleValue))
+                throw new InvalidOperationException("Loader probe reported an invalid handle: " + handleText);
+            bool concurrentHandleStable;
+            if (!bool.TryParse(stableText, out concurrentHandleStable))
+                throw new InvalidOperationException("Loader probe reported an invalid concurrency flag: " + stableText);
+
+            return new ProbeResult(status, version, new IntPtr(handleValue), diagnostic, concurrentHandleStable, output);
+        }
+
+        private static string ReadValue(string output, string prefix)
+        {
+            string[] lines = output.Replace("\r", string.Empty).Split('\n');
+            for (int index = 0; index < lines.Length; index++)
+            {
+                if (lines[index].StartsWith(prefix, StringComparison.Ordinal))
+                    return lines[index].Substring(prefix.Length);
+            }
+
+            throw new InvalidOperationException("Loader probe did not report " + prefix + ". Output: " + output);
+        }
+    }
+}
@@ -7,9 +7,10 @@ internal static class Program
    private static int Main(string[] args)
    {
        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
-            args[0] != "corridor" && args[0] != "optimization" && args[0] != "all-foundation"))
+            args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
+            args[0] != "all-foundation"))
        {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|all-foundation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp-loader|all-foundation");
            return 2;
        }

@@ -40,6 +41,15 @@ internal static class Program
                MultiWheelC.TrajectoryPlanning.EMPlanner.OptimizationChecks.Run();
                Console.WriteLine("PASS optimization");
            }
+            if (args[0] == "osqp-loader")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.Run();
+                Console.WriteLine("PASS osqp-loader");
+            }
+            if (args[0] == "osqp-probe")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
+            }
            return 0;
        }
        catch (Exception exception)