✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成

* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口) * 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装) * 更新 ControllerClientCompat 适配新的规划接口 * 添加构建脚本 (Copy-NativePlanningArtifacts.ps1) * 新增互操作测试及一致性对比测试 * 更新解决方案文件和设计文档
2026-06-23 15:36:01 +08:00
parent bb21e0923d
commit 9c56ee825e
30 changed files with 2674 additions and 2 deletions
--- a/src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj
+++ b/src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj
@@ -0,0 +1,15 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
+    <ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
+  </ItemGroup>
+
+</Project>
--- a/src/Flyshot.Native.Planning/NativeIcspInteropModels.cs
+++ b/src/Flyshot.Native.Planning/NativeIcspInteropModels.cs
@@ -0,0 +1,77 @@
+using System.Runtime.InteropServices;
+
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 与 Rust <c>FlyshotIcspOptions</c> 保持逐字段一致的 C ABI 选项结构。
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+internal struct NativeIcspOptions
+{
+    /// <summary>
+    /// 收敛指标停止阈值。
+    /// </summary>
+    internal double Threshold;
+
+    /// <summary>
+    /// 最大迭代索引。
+    /// </summary>
+    internal int MaxIterations;
+
+    /// <summary>
+    /// 非零表示启用最终 scale 检查。
+    /// </summary>
+    internal int EnforceFinalScale;
+
+    /// <summary>
+    /// 最终 scale 允许的非负容差。
+    /// </summary>
+    internal double FinalScaleTolerance;
+}
+
+/// <summary>
+/// 与 Rust <c>FlyshotIcspResult</c> 保持逐字段一致的 C ABI 结果结构。
+/// </summary>
+[StructLayout(LayoutKind.Sequential)]
+internal struct NativeIcspResult
+{
+    /// <summary>
+    /// 路点绝对时间连续数组指针。
+    /// </summary>
+    internal nint WaypointTimes;
+
+    /// <summary>
+    /// 路点绝对时间数组长度。
+    /// </summary>
+    internal int WaypointTimesLength;
+
+    /// <summary>
+    /// 分段时长连续数组指针。
+    /// </summary>
+    internal nint SegmentDurations;
+
+    /// <summary>
+    /// 分段时长数组长度。
+    /// </summary>
+    internal int SegmentDurationsLength;
+
+    /// <summary>
+    /// 分段缩放连续数组指针。
+    /// </summary>
+    internal nint SegmentScales;
+
+    /// <summary>
+    /// 分段缩放数组长度。
+    /// </summary>
+    internal int SegmentScalesLength;
+
+    /// <summary>
+    /// 历史最优结果对应的迭代轮数。
+    /// </summary>
+    internal int Iterations;
+
+    /// <summary>
+    /// 历史最小收敛指标。
+    /// </summary>
+    internal double Threshold;
+}
--- a/src/Flyshot.Native.Planning/NativeIcspMethods.cs
+++ b/src/Flyshot.Native.Planning/NativeIcspMethods.cs
@@ -0,0 +1,130 @@
+using System.Runtime.InteropServices;
+
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 声明 Rust 原生规划库导出的稳定 C ABI 方法。
+/// </summary>
+internal static partial class NativeIcspMethods
+{
+    /// <summary>
+    /// 保护原生库句柄首次初始化，避免并发规划重复加载。
+    /// </summary>
+    private static readonly object LibraryLoadLock = new();
+
+    /// <summary>
+    /// 保存已加载原生库句柄，使 P/Invoke 生命周期覆盖整个进程。
+    /// </summary>
+    private static nint _libraryHandle;
+
+    /// <summary>
+    /// 保存当前进程已绑定到 P/Invoke 逻辑名称的实际库路径。
+    /// </summary>
+    private static string? _loadedLibraryPath;
+
+    /// <summary>
+    /// 初始化程序集级 DllImport 解析器，把逻辑库名绑定到显式加载句柄。
+    /// </summary>
+    static NativeIcspMethods()
+    {
+        NativeLibrary.SetDllImportResolver(
+            typeof(NativeIcspMethods).Assembly,
+            ResolveImportedLibrary);
+    }
+
+    /// <summary>
+    /// 使用指定解析器确保原生规划库已加载。
+    /// </summary>
+    /// <param name="resolver">负责生成平台路径并加载原生库的解析器。</param>
+    internal static void EnsureLibraryLoaded(NativePlanningLibraryResolver resolver)
+    {
+        ArgumentNullException.ThrowIfNull(resolver);
+        string requestedPath = resolver.ResolveLibraryPath();
+
+        lock (LibraryLoadLock)
+        {
+            if (_libraryHandle != nint.Zero)
+            {
+                StringComparison comparison = OperatingSystem.IsWindows()
+                    ? StringComparison.OrdinalIgnoreCase
+                    : StringComparison.Ordinal;
+                if (!string.Equals(_loadedLibraryPath, requestedPath, comparison))
+                {
+                    throw new NativePlanningException(
+                        $"Rust 原生规划库已绑定到“{_loadedLibraryPath}”，不能在同一进程中改为“{requestedPath}”。",
+                        errorCode: -1008);
+                }
+
+                return;
+            }
+
+            _libraryHandle = resolver.LoadLibrary();
+            _loadedLibraryPath = requestedPath;
+        }
+    }
+
+    /// <summary>
+    /// 调用 Rust ICSP 入口并返回错误码和结果指针。
+    /// </summary>
+    /// <param name="waypoints">行优先连续路点数组。</param>
+    /// <param name="waypointCount">路点数量。</param>
+    /// <param name="degreesOfFreedom">每个路点的关节自由度。</param>
+    /// <param name="velocityLimits">逐关节速度上限。</param>
+    /// <param name="accelerationLimits">逐关节加速度上限。</param>
+    /// <param name="jerkLimits">逐关节 jerk 上限。</param>
+    /// <param name="options">与 Rust C ABI 对齐的规划选项。</param>
+    /// <param name="result">接收 Rust 分配的结果指针。</param>
+    /// <returns>零表示成功，非零表示固定类别的原生错误。</returns>
+    [LibraryImport(
+        NativePlanningLibraryResolver.LogicalLibraryName,
+        EntryPoint = "flyshot_icsp_plan")]
+    internal static unsafe partial int FlyshotIcspPlan(
+        double* waypoints,
+        int waypointCount,
+        int degreesOfFreedom,
+        double* velocityLimits,
+        double* accelerationLimits,
+        double* jerkLimits,
+        NativeIcspOptions* options,
+        out nint result);
+
+    /// <summary>
+    /// 释放由 Rust 分配的 ICSP 结果对象。
+    /// </summary>
+    /// <param name="result">Rust 返回的结果对象指针；允许为空指针。</param>
+    [LibraryImport(
+        NativePlanningLibraryResolver.LogicalLibraryName,
+        EntryPoint = "flyshot_icsp_result_free")]
+    internal static partial void FlyshotIcspResultFree(nint result);
+
+    /// <summary>
+    /// 获取当前线程最近一次 Rust 调用保存的 UTF-8 错误文本。
+    /// </summary>
+    /// <returns>指向线程局部 C 字符串的只读指针。</returns>
+    [LibraryImport(
+        NativePlanningLibraryResolver.LogicalLibraryName,
+        EntryPoint = "flyshot_last_error_message")]
+    internal static partial nint FlyshotLastErrorMessage();
+
+    /// <summary>
+    /// 将固定逻辑库名解析为已经显式加载的系统句柄。
+    /// </summary>
+    /// <param name="libraryName">P/Invoke 请求的逻辑库名。</param>
+    /// <param name="assembly">发起请求的托管程序集。</param>
+    /// <param name="searchPath">调用方提供的可选搜索路径。</param>
+    /// <returns>逻辑名称匹配时返回已加载句柄，否则返回零交给系统继续解析。</returns>
+    private static nint ResolveImportedLibrary(
+        string libraryName,
+        System.Reflection.Assembly assembly,
+        DllImportSearchPath? searchPath)
+    {
+        _ = assembly;
+        _ = searchPath;
+        return string.Equals(
+            libraryName,
+            NativePlanningLibraryResolver.LogicalLibraryName,
+            StringComparison.Ordinal)
+            ? _libraryHandle
+            : nint.Zero;
+    }
+}
--- a/src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs
+++ b/src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs
@@ -0,0 +1,24 @@
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 描述 Rust ICSP 规划器使用的收敛和最终约束选项。
+/// </summary>
+/// <param name="Threshold">收敛指标 <c>sum(abs(scale - 1))</c> 的停止阈值。</param>
+/// <param name="MaxIterations">最大迭代索引；执行范围包含该索引对应轮次。</param>
+/// <param name="EnforceFinalScale">是否拒绝历史最优结果中仍然超限的段。</param>
+/// <param name="FinalScaleTolerance">最终段缩放因子相对 1.0 的非负容差。</param>
+public sealed record NativeIcspPlannerOptions(
+    double Threshold,
+    int MaxIterations,
+    bool EnforceFinalScale,
+    double FinalScaleTolerance)
+{
+    /// <summary>
+    /// 获取与现有托管 <c>ICspPlanner</c> 一致的默认选项。
+    /// </summary>
+    public static NativeIcspPlannerOptions Default { get; } = new(
+        Threshold: 1e-3,
+        MaxIterations: 1000,
+        EnforceFinalScale: true,
+        FinalScaleTolerance: 1e-2);
+}
--- a/src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs
+++ b/src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs
@@ -0,0 +1,37 @@
+using Microsoft.Win32.SafeHandles;
+
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 封装 Rust 分配的 ICSP 结果对象，确保托管异常路径也会调用原生释放函数。
+/// </summary>
+internal sealed class NativeIcspSafeHandle : SafeHandleZeroOrMinusOneIsInvalid
+{
+    /// <summary>
+    /// 初始化一个尚未持有原生结果的安全句柄。
+    /// </summary>
+    public NativeIcspSafeHandle()
+        : base(ownsHandle: true)
+    {
+    }
+
+    /// <summary>
+    /// 初始化并接管 Rust 返回的非零结果指针。
+    /// </summary>
+    /// <param name="result">由 <c>flyshot_icsp_plan</c> 分配的结果指针。</param>
+    internal NativeIcspSafeHandle(nint result)
+        : this()
+    {
+        SetHandle(result);
+    }
+
+    /// <summary>
+    /// 调用 Rust 导出的释放函数归还结果及其连续数组内存。
+    /// </summary>
+    /// <returns>释放函数调用完成后返回 <see langword="true"/>。</returns>
+    protected override bool ReleaseHandle()
+    {
+        NativeIcspMethods.FlyshotIcspResultFree(handle);
+        return true;
+    }
+}
--- a/src/Flyshot.Native.Planning/NativePlanningException.cs
+++ b/src/Flyshot.Native.Planning/NativePlanningException.cs
@@ -0,0 +1,24 @@
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 表示原生规划库加载、调用或数值计算失败。
+/// </summary>
+public sealed class NativePlanningException : Exception
+{
+    /// <summary>
+    /// 初始化包含原生错误码和底层异常的规划异常。
+    /// </summary>
+    /// <param name="message">供调用方诊断的错误说明。</param>
+    /// <param name="errorCode">Rust C ABI 返回的错误码；托管加载错误使用负数。</param>
+    /// <param name="innerException">触发本异常的底层异常；没有时为 <see langword="null"/>。</param>
+    public NativePlanningException(string message, int errorCode, Exception? innerException = null)
+        : base(message, innerException)
+    {
+        ErrorCodeValue = errorCode;
+    }
+
+    /// <summary>
+    /// 获取 Rust C ABI 错误码或托管互操作错误码。
+    /// </summary>
+    public int ErrorCodeValue { get; }
+}
--- a/src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs
+++ b/src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs
@@ -0,0 +1,104 @@
+using System.Runtime.InteropServices;
+
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 描述当前进程加载原生规划库时使用的平台信息。
+/// </summary>
+/// <param name="OperatingSystem">当前操作系统名称。</param>
+/// <param name="Architecture">当前进程体系结构。</param>
+public readonly record struct NativePlanningPlatform(string OperatingSystem, Architecture Architecture);
+
+/// <summary>
+/// 按当前操作系统和体系结构解析并加载 Rust 原生规划库。
+/// </summary>
+public sealed class NativePlanningLibraryResolver
+{
+    /// <summary>
+    /// 原生规划库的逻辑名称。
+    /// </summary>
+    public const string LogicalLibraryName = "flyshot_native_planning";
+
+    private readonly Func<NativePlanningPlatform, string> _resolveLibraryPath;
+
+    /// <summary>
+    /// 初始化使用默认部署目录解析规则的原生库解析器。
+    /// </summary>
+    public NativePlanningLibraryResolver()
+        : this(ResolveDefaultLibraryPath)
+    {
+    }
+
+    /// <summary>
+    /// 初始化使用自定义路径解析规则的原生库解析器。
+    /// </summary>
+    /// <param name="resolveLibraryPath">根据当前平台返回原生库路径或名称的函数。</param>
+    public NativePlanningLibraryResolver(Func<NativePlanningPlatform, string> resolveLibraryPath)
+    {
+        _resolveLibraryPath = resolveLibraryPath ?? throw new ArgumentNullException(nameof(resolveLibraryPath));
+    }
+
+    /// <summary>
+    /// 解析当前平台应加载的原生库路径。
+    /// </summary>
+    /// <returns>原生库路径或可被系统加载器识别的库名称。</returns>
+    public string ResolveLibraryPath()
+    {
+        string operatingSystem = OperatingSystem.IsWindows()
+            ? "Windows"
+            : OperatingSystem.IsLinux()
+                ? "Linux"
+                : "Unsupported";
+        return _resolveLibraryPath(new NativePlanningPlatform(operatingSystem, RuntimeInformation.ProcessArchitecture));
+    }
+
+    /// <summary>
+    /// 加载当前平台的原生规划库并返回系统句柄。
+    /// </summary>
+    /// <returns>由 <see cref="NativeLibrary"/> 管理的非零库句柄。</returns>
+    /// <exception cref="NativePlanningException">库缺失、架构不匹配或系统不受支持时抛出。</exception>
+    public nint LoadLibrary()
+    {
+        string path = ResolveLibraryPath();
+        try
+        {
+            return NativeLibrary.Load(path);
+        }
+        catch (Exception exception) when (
+            exception is DllNotFoundException
+            or BadImageFormatException
+            or FileLoadException)
+        {
+            throw new NativePlanningException(
+                $"无法加载 Rust 原生规划库“{path}”。请确认库文件存在且与 {RuntimeInformation.ProcessArchitecture} 架构匹配。",
+                errorCode: -1001,
+                exception);
+        }
+    }
+
+    /// <summary>
+    /// 根据当前平台生成与托管程序集同目录的默认原生库路径。
+    /// </summary>
+    /// <param name="platform">当前操作系统和进程体系结构。</param>
+    /// <returns>默认原生库绝对路径。</returns>
+    private static string ResolveDefaultLibraryPath(NativePlanningPlatform platform)
+    {
+        if (platform.Architecture != Architecture.X64)
+        {
+            throw new NativePlanningException(
+                $"Rust 原生规划库第一阶段仅支持 x64，当前进程架构为 {platform.Architecture}。",
+                errorCode: -1002);
+        }
+
+        string fileName = platform.OperatingSystem switch
+        {
+            "Windows" => "flyshot_native_planning.dll",
+            "Linux" => "libflyshot_native_planning.so",
+            _ => throw new NativePlanningException(
+                $"Rust 原生规划库不支持当前操作系统：{platform.OperatingSystem}。",
+                errorCode: -1003)
+        };
+
+        return Path.Combine(AppContext.BaseDirectory, fileName);
+    }
+}
--- a/src/Flyshot.Native.Planning/RustIcspPlanner.cs
+++ b/src/Flyshot.Native.Planning/RustIcspPlanner.cs
@@ -0,0 +1,220 @@
+using System.Runtime.InteropServices;
+using Flyshot.Core.Domain;
+using Flyshot.Core.Planning;
+
+namespace Flyshot.Native.Planning;
+
+/// <summary>
+/// 通过稳定 C ABI 调用 Rust ICSP 核心，并把结果转换为现有托管轨迹模型。
+/// </summary>
+public sealed class RustIcspPlanner
+{
+    private readonly NativeIcspPlannerOptions _options;
+    private readonly NativePlanningLibraryResolver _libraryResolver;
+
+    /// <summary>
+    /// 初始化 Rust ICSP 规划器。
+    /// </summary>
+    /// <param name="options">与托管 ICSP 对齐的收敛及最终约束选项。</param>
+    /// <param name="libraryResolver">负责解析 Windows/Linux x64 原生库的加载器。</param>
+    public RustIcspPlanner(
+        NativeIcspPlannerOptions? options = null,
+        NativePlanningLibraryResolver? libraryResolver = null)
+    {
+        _options = options ?? NativeIcspPlannerOptions.Default;
+        _libraryResolver = libraryResolver ?? new NativePlanningLibraryResolver();
+        ValidateOptions(_options);
+    }
+
+    /// <summary>
+    /// 使用 Rust 原生核心规划给定关节路点。
+    /// </summary>
+    /// <param name="request">包含机器人约束和示教路点的规划请求。</param>
+    /// <returns>与现有托管规划链路兼容的轨迹结果。</returns>
+    /// <exception cref="NativePlanningException">原生库加载或规划调用失败时抛出。</exception>
+    public unsafe PlannedTrajectory Plan(TrajectoryRequest request)
+    {
+        ArgumentNullException.ThrowIfNull(request);
+        ValidateRequest(request);
+        NativeIcspMethods.EnsureLibraryLoaded(_libraryResolver);
+
+        int waypointCount = request.Program.Waypoints.Count;
+        int degreesOfFreedom = request.Robot.DegreesOfFreedom;
+        double[] waypoints = FlattenWaypoints(request, waypointCount, degreesOfFreedom);
+        double[] velocityLimits = request.Robot.JointLimits
+            .Select(limit => limit.VelocityLimit)
+            .ToArray();
+        double[] accelerationLimits = request.Robot.JointLimits
+            .Select(limit => limit.AccelerationLimit)
+            .ToArray();
+        double[] jerkLimits = request.Robot.JointLimits
+            .Select(limit => limit.JerkLimit)
+            .ToArray();
+        var nativeOptions = new NativeIcspOptions
+        {
+            Threshold = _options.Threshold,
+            MaxIterations = _options.MaxIterations,
+            EnforceFinalScale = _options.EnforceFinalScale ? 1 : 0,
+            FinalScaleTolerance = _options.FinalScaleTolerance
+        };
+
+        fixed (double* waypointsPointer = waypoints)
+        fixed (double* velocityLimitsPointer = velocityLimits)
+        fixed (double* accelerationLimitsPointer = accelerationLimits)
+        fixed (double* jerkLimitsPointer = jerkLimits)
+        {
+            int errorCode = NativeIcspMethods.FlyshotIcspPlan(
+                waypointsPointer,
+                waypointCount,
+                degreesOfFreedom,
+                velocityLimitsPointer,
+                accelerationLimitsPointer,
+                jerkLimitsPointer,
+                &nativeOptions,
+                out nint resultPointer);
+            if (errorCode != 0)
+            {
+                string errorMessage = ReadLastErrorMessage();
+                throw new NativePlanningException(
+                    $"Rust ICSP 规划失败，错误码={errorCode}：{errorMessage}",
+                    errorCode);
+            }
+
+            if (resultPointer == nint.Zero)
+            {
+                throw new NativePlanningException(
+                    "Rust ICSP 返回成功，但结果指针为空。",
+                    errorCode: -1005);
+            }
+
+            using var resultHandle = new NativeIcspSafeHandle(resultPointer);
+            NativeIcspResult nativeResult = Marshal.PtrToStructure<NativeIcspResult>(resultPointer);
+            double[] waypointTimes = CopyNativeArray(
+                nativeResult.WaypointTimes,
+                nativeResult.WaypointTimesLength,
+                "waypoint_times");
+            double[] segmentDurations = CopyNativeArray(
+                nativeResult.SegmentDurations,
+                nativeResult.SegmentDurationsLength,
+                "segment_durations");
+            double[] segmentScales = CopyNativeArray(
+                nativeResult.SegmentScales,
+                nativeResult.SegmentScalesLength,
+                "segment_scales");
+
+            return new PlannedTrajectory(
+                robot: request.Robot,
+                originalProgram: request.Program,
+                plannedWaypoints: request.Program.Waypoints,
+                waypointTimes: waypointTimes,
+                segmentDurations: segmentDurations,
+                segmentScales: segmentScales,
+                method: PlanningMethod.IcspRust,
+                iterations: nativeResult.Iterations,
+                threshold: nativeResult.Threshold);
+        }
+    }
+
+    /// <summary>
+    /// 校验托管规划选项，防止非法数值穿过 C ABI。
+    /// </summary>
+    /// <param name="options">待校验的原生规划选项。</param>
+    private static void ValidateOptions(NativeIcspPlannerOptions options)
+    {
+        if (!double.IsFinite(options.Threshold) || options.Threshold <= 0.0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(options), "收敛阈值必须为有限正数。");
+        }
+        if (options.MaxIterations < 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(options), "最大迭代次数不能为负数。");
+        }
+        if (!double.IsFinite(options.FinalScaleTolerance) || options.FinalScaleTolerance < 0.0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(options), "最终 scale 容差必须为有限非负数。");
+        }
+    }
+
+    /// <summary>
+    /// 校验规划请求与机器人自由度，确保扁平数组布局可安全建立。
+    /// </summary>
+    /// <param name="request">待传入 Rust 的规划请求。</param>
+    private static void ValidateRequest(TrajectoryRequest request)
+    {
+        if (request.Program.Waypoints.Count < 4)
+        {
+            throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
+        }
+        int degreesOfFreedom = request.Robot.DegreesOfFreedom;
+        if (request.Program.Waypoints.Any(waypoint => waypoint.Positions.Count != degreesOfFreedom))
+        {
+            throw new ArgumentException("所有路点维度必须与机器人自由度一致。", nameof(request));
+        }
+    }
+
+    /// <summary>
+    /// 将托管路点按行优先顺序复制为连续数组。
+    /// </summary>
+    /// <param name="request">包含路点的规划请求。</param>
+    /// <param name="waypointCount">已校验的路点数量。</param>
+    /// <param name="degreesOfFreedom">已校验的机器人自由度。</param>
+    /// <returns>长度为路点数量乘自由度的连续数组。</returns>
+    private static double[] FlattenWaypoints(
+        TrajectoryRequest request,
+        int waypointCount,
+        int degreesOfFreedom)
+    {
+        var flattened = new double[checked(waypointCount * degreesOfFreedom)];
+        for (int waypoint = 0; waypoint < waypointCount; waypoint++)
+        {
+            for (int joint = 0; joint < degreesOfFreedom; joint++)
+            {
+                flattened[waypoint * degreesOfFreedom + joint] =
+                    request.Program.Waypoints[waypoint].Positions[joint];
+            }
+        }
+        return flattened;
+    }
+
+    /// <summary>
+    /// 将 Rust 连续数组复制到独立托管数组，避免结果释放后继续引用原生内存。
+    /// </summary>
+    /// <param name="pointer">Rust 结果中的连续数组指针。</param>
+    /// <param name="length">数组元素数量。</param>
+    /// <param name="fieldName">用于异常诊断的 ABI 字段名。</param>
+    /// <returns>与原生结果生命周期解耦的托管数组。</returns>
+    private static double[] CopyNativeArray(nint pointer, int length, string fieldName)
+    {
+        if (length < 0)
+        {
+            throw new NativePlanningException(
+                $"Rust ICSP 结果字段 {fieldName} 的长度为负数：{length}。",
+                errorCode: -1006);
+        }
+        if (length > 0 && pointer == nint.Zero)
+        {
+            throw new NativePlanningException(
+                $"Rust ICSP 结果字段 {fieldName} 长度为 {length}，但指针为空。",
+                errorCode: -1007);
+        }
+
+        var values = new double[length];
+        if (length > 0)
+        {
+            Marshal.Copy(pointer, values, 0, length);
+        }
+        return values;
+    }
+
+    /// <summary>
+    /// 读取 Rust 当前线程保存的最近错误文本。
+    /// </summary>
+    /// <returns>UTF-8 错误文本；空指针时返回占位诊断。</returns>
+    private static string ReadLastErrorMessage()
+    {
+        nint messagePointer = NativeIcspMethods.FlyshotLastErrorMessage();
+        return messagePointer == nint.Zero
+            ? "原生库未提供错误文本"
+            : Marshal.PtrToStringUTF8(messagePointer) ?? "原生错误文本不是有效 UTF-8";
+    }
+}