From 39c1708c4847937bc6879fb4e25904430b685dfa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Mon, 3 Aug 2026 23:48:41 +0800 Subject: [PATCH] feat: load pinned OSQP native library --- .../Optimization/Osqp/OsqpNativeLoader.cs | 110 ++++++++++ .../Optimization/Osqp/OsqpNativeMethods.cs | 111 ++++++++++ .../Optimization/Osqp/OsqpNativeStructures.cs | 205 ++++++++++++++++++ .../EMPlannerVerificationHost/OsqpChecks.cs | 170 +++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 14 +- 5 files changed, 608 insertions(+), 2 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeLoader.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeMethods.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeStructures.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeLoader.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeLoader.cs new file mode 100644 index 0000000..6f9237f --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeLoader.cs @@ -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")); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeMethods.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeMethods.cs new file mode 100644 index 0000000..9a28b6f --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeMethods.cs @@ -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(moduleHandle, "osqp_version"), + Resolve(moduleHandle, "osqp_set_default_settings"), + Resolve(moduleHandle, "osqp_setup"), + Resolve(moduleHandle, "osqp_warm_start"), + Resolve(moduleHandle, "osqp_solve"), + Resolve(moduleHandle, "osqp_cleanup")); + } + + private static T Resolve(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(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; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeStructures.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeStructures.cs new file mode 100644 index 0000000..0213135 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeStructures.cs @@ -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); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs new file mode 100644 index 0000000..2333602 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs @@ -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(); + 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); + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 68e49f6..fd07a15 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -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)