First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: load pinned OSQP native library

Browse Source
This commit is contained in:
梁薄云
2026-08-03 23:48:41 +08:00
parent 2abb465d98
commit 39c1708c48
5 changed files with 608 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs
+170
View File
@@ -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);
}
}
}
ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
+12 -2
View File
@@ -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)