111 lines
4.2 KiB
C#
111 lines
4.2 KiB
C#
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"));
|
|
}
|
|
}
|