From 9c56ee825efbdd7465d0cdc9892af33281303ae1 Mon Sep 17 00:00:00 2001 From: "yunxiao.zhu" Date: Tue, 23 Jun 2026 15:36:01 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat(native-planning):=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=20Rust=20ICSP=20=E5=8E=9F=E7=94=9F=E8=A7=84=E5=88=92?= =?UTF-8?q?=E6=A0=B8=E5=BF=83=E5=8F=8A=20C#=20=E4=BA=92=E6=93=8D=E4=BD=9C?= =?UTF-8?q?=E9=9B=86=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口) * 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装) * 更新 ControllerClientCompat 适配新的规划接口 * 添加构建脚本 (Copy-NativePlanningArtifacts.ps1) * 新增互操作测试及一致性对比测试 * 更新解决方案文件和设计文档 --- .gitignore | 3 + FlyshotReplacement.sln | 15 + README.md | 25 + build/Copy-NativePlanningArtifacts.ps1 | 94 ++++ ...6-06-22-rust-icsp-native-implementation.md | 330 +++++++++++++ .../2026-06-22-rust-icsp-native-design.md | 20 + native/flyshot-native-planning/Cargo.lock | 7 + native/flyshot-native-planning/Cargo.toml | 13 + native/flyshot-native-planning/src/error.rs | 33 ++ native/flyshot-native-planning/src/ffi.rs | 436 ++++++++++++++++++ native/flyshot-native-planning/src/icsp.rs | 367 +++++++++++++++ native/flyshot-native-planning/src/lib.rs | 6 + native/flyshot-native-planning/src/spline.rs | 277 +++++++++++ .../ControllerClientTrajectoryOrchestrator.cs | 6 + .../Flyshot.ControllerClientCompat.csproj | 1 + .../FlyshotExecutionOptions.cs | 2 +- .../TrajectoryExecutionOptions.cs | 2 +- src/Flyshot.Core.Domain/TrajectoryRequest.cs | 5 + .../Flyshot.Native.Planning.csproj | 15 + .../NativeIcspInteropModels.cs | 77 ++++ .../NativeIcspMethods.cs | 130 ++++++ .../NativeIcspPlannerOptions.cs | 24 + .../NativeIcspSafeHandle.cs | 37 ++ .../NativePlanningException.cs | 24 + .../NativePlanningLibraryResolver.cs | 104 +++++ .../RustIcspPlanner.cs | 220 +++++++++ .../Flyshot.Core.Tests.csproj | 1 + .../RuntimeOrchestrationTests.cs | 107 +++++ .../RustIcspPlannerInteropTests.cs | 135 ++++++ .../RustIcspPlannerParityTests.cs | 160 +++++++ 30 files changed, 2674 insertions(+), 2 deletions(-) create mode 100644 build/Copy-NativePlanningArtifacts.ps1 create mode 100644 docs/superpowers/plans/2026-06-22-rust-icsp-native-implementation.md create mode 100644 native/flyshot-native-planning/Cargo.lock create mode 100644 native/flyshot-native-planning/Cargo.toml create mode 100644 native/flyshot-native-planning/src/error.rs create mode 100644 native/flyshot-native-planning/src/ffi.rs create mode 100644 native/flyshot-native-planning/src/icsp.rs create mode 100644 native/flyshot-native-planning/src/lib.rs create mode 100644 native/flyshot-native-planning/src/spline.rs create mode 100644 src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj create mode 100644 src/Flyshot.Native.Planning/NativeIcspInteropModels.cs create mode 100644 src/Flyshot.Native.Planning/NativeIcspMethods.cs create mode 100644 src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs create mode 100644 src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs create mode 100644 src/Flyshot.Native.Planning/NativePlanningException.cs create mode 100644 src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs create mode 100644 src/Flyshot.Native.Planning/RustIcspPlanner.cs create mode 100644 tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs create mode 100644 tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs diff --git a/.gitignore b/.gitignore index c88f4a3..560658c 100644 --- a/.gitignore +++ b/.gitignore @@ -405,3 +405,6 @@ codex-dotnet-home/* outputs/obfuscated/ outputs/obfuscation-maps/ .tmp/obfuscation/ + +# Rust 原生规划库构建产物 +**/target/ diff --git a/FlyshotReplacement.sln b/FlyshotReplacement.sln index c4901e0..3e115ad 100644 --- a/FlyshotReplacement.sln +++ b/FlyshotReplacement.sln @@ -27,6 +27,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.ControllerClientCom EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Native.Planning", "src\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj", "{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -157,6 +159,18 @@ Global {B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.Build.0 = Release|Any CPU {B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.ActiveCfg = Release|Any CPU {B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.Build.0 = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|Any CPU.Build.0 = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x64.ActiveCfg = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x64.Build.0 = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x86.ActiveCfg = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Debug|x86.Build.0 = Debug|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|Any CPU.ActiveCfg = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|Any CPU.Build.0 = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x64.ActiveCfg = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x64.Build.0 = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x86.ActiveCfg = Release|Any CPU + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0}.Release|x86.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE @@ -172,5 +186,6 @@ Global {E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} {5B45CC23-3551-4D0F-B3CC-22659C2A8BA3} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} {B705FA6C-19CA-44A8-882C-6CE26A5379C9} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} + {928CF396-C0E1-488E-BCC1-CF0279CF2DD0} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} EndGlobalSection EndGlobal diff --git a/README.md b/README.md index c2af03a..3507e6d 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ - 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。 - `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。 - `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。 +- `Flyshot.Native.Planning` 已提供显式 Rust ICSP 规划入口，通过 C ABI 调用 `flyshot_native_planning`；普通轨迹和飞拍轨迹可使用准确参数 `method="icsp_rust"` 选择该后端，默认 `icsp` 仍使用 C# `ICspPlanner`。 - `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。 - 2026-04-28 UTTC 抓包确认：UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`，`JointDetialTraj.txt` 为弧度制 `rad`，`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹。 - 2026-04-30 本机 `50001/TCP+JSON` 抓包确认：`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数，不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路，新系统按 replacement-only 内部参数限制规划加速度。 @@ -66,6 +67,7 @@ - [x] 落地配置兼容与机器人模型解析 - [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴 - [x] 完成 ICSP 轨迹导出结果与旧系统对齐 +- [x] 增加 Rust `cdylib` ICSP 核心、C# P/Invoke 适配和 C#/Rust 自动对拍 - [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路 - [x] 落地 Web 状态页 - [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页（`/debug`），与 `Swagger:Enabled` 同步可见 @@ -79,6 +81,29 @@ - [x] 完成真机优先与离线飞拍规划改造：`RealReady / OfflineReady / ModelLoadFailed` 状态、固定轨迹资产、完整离线规划响应和 Debug 页面 - [x] 删除生产仿真切换 HTTP 入口及旧 `/init_mpc_robt`，统一使用 `/init_mpc_robot` +### Rust ICSP 原生库构建 + +Rust 规划器通过普通轨迹或飞拍轨迹的准确参数 `method="icsp_rust"` 显式选择，不替换默认 `icsp` 使用的 C# `ICspPlanner`，也不会在原生库缺失时静默回退。 + +Windows x64 需要安装： + +- `rustup` 的 `stable-x86_64-pc-windows-msvc` 工具链。 +- Visual Studio 或 Visual Studio Build Tools 的 C++ x64 编译工具和 Windows SDK。 + +在仓库根目录执行： + +```powershell +.\build\Copy-NativePlanningArtifacts.ps1 -Configuration Release +``` + +脚本会通过 `vcvars64.bat` 初始化 MSVC 环境，构建 +`native/flyshot-native-planning/target/release/flyshot_native_planning.dll`， +并默认复制到 `tests/Flyshot.Core.Tests/bin/Release/net8.0/`。发布服务时可通过 +`-DestinationDirectories` 把原生库复制到实际发布目录。 + +Linux x64 在已安装 `stable-x86_64-unknown-linux-gnu` 工具链的 Linux 环境执行同一脚本， +产物为 `libflyshot_native_planning.so`。Windows 第一阶段不要求交叉编译 Linux 原生库。 + 剩余 Todo： 1. 配置与测试基线 diff --git a/build/Copy-NativePlanningArtifacts.ps1 b/build/Copy-NativePlanningArtifacts.ps1 new file mode 100644 index 0000000..91e9e5b --- /dev/null +++ b/build/Copy-NativePlanningArtifacts.ps1 @@ -0,0 +1,94 @@ +param( + [ValidateSet("Debug", "Release")] + [string]$Configuration = "Release", + + [string[]]$DestinationDirectories = @() +) + +$ErrorActionPreference = "Stop" + +# 从脚本位置定位仓库根目录，确保从任意工作目录调用都得到一致结果。 +$repositoryRoot = Split-Path -Parent $PSScriptRoot +$manifestPath = Join-Path $repositoryRoot "native/flyshot-native-planning/Cargo.toml" +$cargoExecutable = if ($IsWindows) { "cargo.exe" } else { "cargo" } +$cargoPath = Join-Path $HOME ".cargo/bin/$cargoExecutable" +if (-not (Test-Path -LiteralPath $cargoPath)) { + throw "未找到 Rust Cargo：$cargoPath。请先通过 rustup 安装 stable-x86_64-pc-windows-msvc。" +} + +$cargoArguments = @("build", "--manifest-path", $manifestPath) +if ($Configuration -eq "Release") { + $cargoArguments += "--release" +} + +if ($IsWindows) { + # Windows MSVC 目标必须在 VS C++ 开发者环境中运行，避免 PATH 中其他 link.exe 抢占。 + $vswherePath = "${env:ProgramFiles(x86)}\Microsoft Visual Studio\Installer\vswhere.exe" + if (-not (Test-Path -LiteralPath $vswherePath)) { + throw "未找到 vswhere.exe，无法定位 Visual Studio C++ Build Tools。" + } + + $installationPath = & $vswherePath -latest -products * ` + -requires Microsoft.VisualStudio.Component.VC.Tools.x86.x64 ` + -property installationPath + if ([string]::IsNullOrWhiteSpace($installationPath)) { + throw "未找到已安装的 Visual Studio C++ x64 编译工具。" + } + + $vcvarsPath = Join-Path $installationPath "VC/Auxiliary/Build/vcvars64.bat" + if (-not (Test-Path -LiteralPath $vcvarsPath)) { + throw "未找到 VS x64 开发者环境脚本：$vcvarsPath" + } + + $quotedArguments = $cargoArguments | ForEach-Object { '"' + $_.Replace('"', '\"') + '"' } + $command = 'call "{0}" >nul && "{1}" {2}' -f ` + $vcvarsPath, $cargoPath, ($quotedArguments -join " ") + & cmd.exe /d /c $command + if ($LASTEXITCODE -ne 0) { + throw "Rust 原生规划库构建失败，退出码：$LASTEXITCODE" + } +} +else { + & $cargoPath @cargoArguments + if ($LASTEXITCODE -ne 0) { + throw "Rust 原生规划库构建失败，退出码：$LASTEXITCODE" + } +} + +$profileDirectory = if ($Configuration -eq "Release") { "release" } else { "debug" } +$libraryName = if ($IsWindows) { + "flyshot_native_planning.dll" +} +elseif ($IsLinux) { + "libflyshot_native_planning.so" +} +else { + throw "当前脚本仅支持 Windows x64 和 Linux x64。" +} +$sourcePath = Join-Path $repositoryRoot "native/flyshot-native-planning/target/$profileDirectory/$libraryName" +if (-not (Test-Path -LiteralPath $sourcePath)) { + throw "Rust 构建已结束，但未找到原生库：$sourcePath" +} + +# 未显式指定目标时，默认复制到对应配置的 Core 测试输出目录。 +if ($DestinationDirectories.Count -eq 0) { + $DestinationDirectories = @( + (Join-Path $repositoryRoot "tests/Flyshot.Core.Tests/bin/$Configuration/net8.0") + ) +} + +foreach ($destinationDirectory in $DestinationDirectories) { + $destinationPath = if ([System.IO.Path]::IsPathRooted($destinationDirectory)) { + $destinationDirectory + } + else { + Join-Path $repositoryRoot $destinationDirectory + } + $resolvedDestination = [System.IO.Path]::GetFullPath( + $destinationPath) + New-Item -ItemType Directory -Force -Path $resolvedDestination | Out-Null + Copy-Item -LiteralPath $sourcePath ` + -Destination (Join-Path $resolvedDestination $libraryName) ` + -Force + Write-Host "已复制原生规划库到：$resolvedDestination" +} diff --git a/docs/superpowers/plans/2026-06-22-rust-icsp-native-implementation.md b/docs/superpowers/plans/2026-06-22-rust-icsp-native-implementation.md new file mode 100644 index 0000000..36e335b --- /dev/null +++ b/docs/superpowers/plans/2026-06-22-rust-icsp-native-implementation.md @@ -0,0 +1,330 @@ +# Rust ICSP Native Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** 为现有 `ICspPlanner` 增加一套基于 Rust `cdylib` 的原生 ICSP 规划实现，并通过 C ABI + C# P/Invoke 完成可验证的对拍接入，同时不改变当前默认生产路径。 + +**Architecture:** 新增 `native/flyshot-native-planning` Rust 原生库承载 clamped-zero 三次样条与 ICSP 迭代核心；新增 `src/Flyshot.Native.Planning` 托管互操作项目负责库加载、数组编排、错误转换和结果复制。现有 `Flyshot.Core.Planning.ICspPlanner` 保持不变，`RustIcspPlanner` 仅作为显式新入口并在测试中与托管实现逐项对拍。 + +**Tech Stack:** Rust `cdylib`、C ABI、.NET 8、`LibraryImport`、`SafeHandle`、xUnit、PowerShell、Cargo + +--- + +### Task 1: 建立原生与托管骨架 + +**Files:** +- Create: `native/flyshot-native-planning/Cargo.toml` +- Create: `native/flyshot-native-planning/src/lib.rs` +- Create: `native/flyshot-native-planning/src/error.rs` +- Create: `native/flyshot-native-planning/src/ffi.rs` +- Create: `native/flyshot-native-planning/src/icsp.rs` +- Create: `native/flyshot-native-planning/src/spline.rs` +- Create: `src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj` +- Create: `src/Flyshot.Native.Planning/NativeIcspMethods.cs` +- Create: `src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs` +- Create: `src/Flyshot.Native.Planning/NativePlanningException.cs` +- Create: `src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs` +- Modify: `FlyshotReplacement.sln` +- Modify: `tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj` + +- [x] **Step 1: 先写托管侧失败测试，锁定缺少原生库时的报错边界** + +```csharp +[Fact] +public void RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing() +{ + var planner = new RustIcspPlanner( + options: NativeIcspPlannerOptions.Default, + libraryResolver: new NativePlanningLibraryResolver(_ => "missing-library")); + + var exception = Assert.Throws(() => planner.Plan(CreateSimpleRequest())); + Assert.Contains("missing-library", exception.Message); +} +``` + +- [x] **Step 2: 运行单测确认当前失败** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing -v minimal` +Expected: FAIL，提示 `RustIcspPlanner` 或互操作类型不存在 + +- [x] **Step 3: 创建 Rust `cdylib` 和托管互操作项目骨架** + +```toml +[package] +name = "flyshot-native-planning" +version = "0.1.0" +edition = "2021" + +[lib] +crate-type = ["cdylib"] + +[profile.release] +lto = true +codegen-units = 1 +strip = "symbols" +panic = "abort" +``` + +```xml + + + net8.0 + true + + + + + + +``` + +- [x] **Step 4: 添加最小库解析与异常包装实现** + +```csharp +public sealed class NativePlanningException : Exception +{ + public NativePlanningException(string message, int errorCode, Exception? innerException = null) + : base(message, innerException) + { + ErrorCodeValue = errorCode; + } + + public int ErrorCodeValue { get; } +} +``` + +```csharp +public sealed class NativePlanningLibraryResolver +{ + private readonly Func _resolveLibraryPath; +} +``` + +- [x] **Step 5: 重新运行单测，确认错误已变为清晰的原生库缺失异常** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing -v minimal` +Expected: PASS + +### Task 2: 先做样条基础能力 + +**Files:** +- Modify: `native/flyshot-native-planning/src/spline.rs` +- Modify: `native/flyshot-native-planning/src/lib.rs` +- Create: `native/flyshot-native-planning/tests/spline_tests.rs` + +- [x] **Step 1: 先写 Rust 失败测试，锁定 clamped-zero 样条边界和值/导数接口** + +```rust +#[test] +fn clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity() { + let times = [0.0, 1.0, 2.0, 3.0]; + let values = [0.0, 1.0, 0.5, 2.0]; + let spline = CubicSpline1D::new_clamped_zero(×, &values).unwrap(); + + assert_abs_diff_eq!(spline.position(0.0), 0.0, epsilon = 1e-12); + assert_abs_diff_eq!(spline.position(3.0), 2.0, epsilon = 1e-12); + assert_abs_diff_eq!(spline.velocity(0.0), 0.0, epsilon = 1e-12); + assert_abs_diff_eq!(spline.velocity(3.0), 0.0, epsilon = 1e-12); +} +``` + +- [x] **Step 2: 运行 Rust 测试确认失败** + +Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml spline_tests::clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity -- --exact` +Expected: FAIL，提示样条类型或构造函数不存在 + +- [x] **Step 3: 实现三对角求解、样条系数和每段导数峰值接口** + +```rust +pub struct CubicSpline1D { + knots: Vec, + coefficients: Vec<[f64; 4]>, +} + +impl CubicSpline1D { + pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result { /* ... */ } + pub fn position(&self, time: f64) -> f64 { /* ... */ } + pub fn velocity(&self, time: f64) -> f64 { /* ... */ } + pub fn acceleration(&self, time: f64) -> f64 { /* ... */ } + pub fn jerk(&self, time: f64) -> f64 { /* ... */ } + pub fn segment_peak_derivatives(&self, segment_index: usize) -> [f64; 3] { /* ... */ } +} +``` + +- [x] **Step 4: 重新运行 Rust 样条测试** + +Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml spline_tests::clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity -- --exact` +Expected: PASS + +### Task 3: 实现纯 Rust ICSP 核心 + +**Files:** +- Modify: `native/flyshot-native-planning/src/icsp.rs` +- Create: `native/flyshot-native-planning/tests/icsp_tests.rs` + +- [x] **Step 1: 先写 Rust 失败测试，锁定初始段时长、scale 计算和未收敛路径** + +```rust +#[test] +fn icsp_returns_same_iteration_count_as_managed_baseline_fixture() { + let input = test_fixture_three_segment_case(); + let result = plan_icsp(&input).unwrap(); + + assert_eq!(result.iterations, 2); + assert_abs_diff_eq!(result.threshold, 0.0, epsilon = 1e-12); +} +``` + +- [x] **Step 2: 运行 Rust ICSP 测试确认失败** + +Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml icsp_tests::icsp_returns_same_iteration_count_as_managed_baseline_fixture -- --exact` +Expected: FAIL，提示 `plan_icsp` 或结果结构不存在 + +- [x] **Step 3: 实现最小 ICSP 纯算法结构，逐项复刻 C# 收敛语义** + +```rust +pub struct IcspInput<'a> { + pub waypoints: &'a [f64], + pub waypoint_count: usize, + pub dof: usize, + pub velocity_limits: &'a [f64], + pub acceleration_limits: &'a [f64], + pub jerk_limits: &'a [f64], + pub options: IcspOptions, +} + +pub fn plan_icsp(input: &IcspInput<'_>) -> Result { /* ... */ } +``` + +- [x] **Step 4: 重新运行 Rust ICSP 测试** + +Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml icsp_tests::icsp_returns_same_iteration_count_as_managed_baseline_fixture -- --exact` +Expected: PASS + +### Task 4: 接通 C ABI 和托管结果复制 + +**Files:** +- Modify: `native/flyshot-native-planning/src/ffi.rs` +- Modify: `native/flyshot-native-planning/src/error.rs` +- Modify: `src/Flyshot.Native.Planning/NativeIcspMethods.cs` +- Modify: `src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs` +- Create: `src/Flyshot.Native.Planning/NativeIcspInteropModels.cs` +- Create: `src/Flyshot.Native.Planning/RustIcspPlanner.cs` +- Create: `tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs` + +- [x] **Step 1: 先写托管失败测试，锁定数组布局、结果复制和释放路径** + +```csharp +[Fact] +public void RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult() +{ + var planner = CreateRustPlannerAgainstBuiltNativeLibrary(); + var result = planner.Plan(CreateSimpleRequest()); + + Assert.Equal(PlanningMethod.Icsp, result.Method); + Assert.Equal(result.PlannedWaypointCount, result.WaypointTimes.Count); +} +``` + +- [x] **Step 2: 运行单测确认失败** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult -v minimal` +Expected: FAIL，提示 P/Invoke 签名、结果结构或释放逻辑未实现 + +- [x] **Step 3: 实现 C ABI 入口、线程局部错误文本和 `SafeHandle` 释放** + +```rust +#[no_mangle] +pub extern "C" fn flyshot_icsp_plan(/* ... */) -> i32 { /* ... */ } + +#[no_mangle] +pub extern "C" fn flyshot_icsp_result_free(result: *mut FlyshotIcspResult) { /* ... */ } + +#[no_mangle] +pub extern "C" fn flyshot_last_error_message() -> *const c_char { /* ... */ } +``` + +```csharp +[LibraryImport(NativeLibraryName, EntryPoint = "flyshot_icsp_plan")] +internal static unsafe partial int FlyshotIcspPlan(/* ... */); +``` + +- [x] **Step 4: 重新运行托管互操作测试** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult -v minimal` +Expected: PASS + +### Task 5: 增加 C#/Rust 对拍测试和构建复制脚本 + +**Files:** +- Modify: `tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs` +- Create: `tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs` +- Create: `build/Copy-NativePlanningArtifacts.ps1` +- Modify: `README.md` + +- [x] **Step 1: 先写失败测试，锁定 C# 与 Rust 在基础样本上的逐项一致** + +```csharp +[Fact] +public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture() +{ + var request = CreateCompatibilityRequest(); + var managed = new ICspPlanner().Plan(request); + var native = CreateRustPlannerAgainstBuiltNativeLibrary().Plan(request); + + AssertTrajectoryMatches(managed, native, tolerance: 1e-9); +} +``` + +- [x] **Step 2: 运行对拍测试确认失败** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture -v minimal` +Expected: FAIL，提示数值未对齐或原生库复制失败 + +- [x] **Step 3: 补齐原生库复制脚本、测试输出复制规则和对拍辅助断言** + +```powershell +param( + [string]$Configuration = "Release" +) + +cargo build --manifest-path native/flyshot-native-planning/Cargo.toml --release +Copy-Item native/flyshot-native-planning/target/release/flyshot_native_planning.dll tests/Flyshot.Core.Tests/bin/$Configuration/net8.0/ -Force +``` + +- [x] **Step 4: 重新运行对拍测试** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture -v minimal` +Expected: PASS + +### Task 6: 解决方案级验证 + +**Files:** +- Modify: `README.md` +- Modify: `docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md` + +- [x] **Step 1: 运行 Rust 全量测试** + +Run: `cargo test --manifest-path native/flyshot-native-planning/Cargo.toml` +Expected: PASS + +- [ ] **Step 2: 运行 Core 测试** + +Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal` +Expected: PASS + +当前验证：新增 Rust 相关测试全部通过；Core 全套仍保留实施前已存在的两项 +`FlyshotExecutionSendSequenceBuilder` speed-ratio 失败，本计划未修改该行为。 + +- [x] **Step 3: 运行解决方案构建** + +Run: `dotnet build FlyshotReplacement.sln -v minimal` +Expected: PASS + +- [x] **Step 4: 更新文档，记录本地构建、库复制和当前上线边界** + +```markdown +- Rust 规划器当前仅显式使用，不替换默认 `ICspPlanner` +- Windows 需要 `flyshot_native_planning.dll` +- Linux 需要 `libflyshot_native_planning.so` +``` diff --git a/docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md b/docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md index ada17ac..8bc0ef2 100644 --- a/docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md +++ b/docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md @@ -249,3 +249,23 @@ Planning:Backend = Managed | Rust - Linux x64 实际加载 `.so` 并完成规划。 - 缺少原生库时能得到明确且可诊断的错误。 - 现有 C# `ICspPlanner` 和上层运行链路行为不变。 + +## 11. 第一阶段落地状态（2026-06-23） + +当前已落地： + +- `native/flyshot-native-planning` Rust `cdylib`，包含 clamped-zero 样条、 + 一至三阶导数解析峰值、ICSP 迭代和稳定 C ABI。 +- `src/Flyshot.Native.Planning` 托管互操作项目，包含显式平台解析、 + `LibraryImport`、`SafeHandle`、错误码转换和结果复制。 +- Rust 样条、ICSP、FFI 测试，以及 C#/Rust `1e-9` 自动对拍测试。 +- `build/Copy-NativePlanningArtifacts.ps1`，用于初始化 Windows MSVC 环境、 + 构建原生库并复制到测试或发布目录。 +- Windows x64 已实际生成并加载 `flyshot_native_planning.dll` 完成规划。 + +当前上线边界保持不变： + +- 默认生产路径继续使用 C# `ICspPlanner`。 +- Rust 规划器只允许显式选择，库缺失、路径不一致或架构不支持时返回明确错误。 +- Linux x64 的源码、库名和构建脚本边界已落地，但 `.so` 的实际加载验证必须在 + Linux x64 构建环境执行后才能标记完成。 diff --git a/native/flyshot-native-planning/Cargo.lock b/native/flyshot-native-planning/Cargo.lock new file mode 100644 index 0000000..01366c1 --- /dev/null +++ b/native/flyshot-native-planning/Cargo.lock @@ -0,0 +1,7 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "flyshot-native-planning" +version = "0.1.0" diff --git a/native/flyshot-native-planning/Cargo.toml b/native/flyshot-native-planning/Cargo.toml new file mode 100644 index 0000000..df0b305 --- /dev/null +++ b/native/flyshot-native-planning/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "flyshot-native-planning" +version = "0.1.0" +edition = "2021" + +[lib] +crate-type = ["cdylib"] + +[profile.release] +lto = true +codegen-units = 1 +strip = "symbols" +panic = "abort" diff --git a/native/flyshot-native-planning/src/error.rs b/native/flyshot-native-planning/src/error.rs new file mode 100644 index 0000000..194d53c --- /dev/null +++ b/native/flyshot-native-planning/src/error.rs @@ -0,0 +1,33 @@ +//! 原生规划错误定义。 + +use std::fmt::{Display, Formatter}; + +/// 表示 Rust 规划核心中可预期、可转换为 C ABI 错误码的失败。 +#[derive(Debug, Clone, PartialEq)] +pub enum NativePlanningError { + /// 表示输入参数不满足算法前置条件。 + InvalidInput(String), + /// 表示数值计算无法继续。 + NumericalFailure(String), + /// 表示规划迭代结束后仍未满足约束。 + NotConverged(String), +} + +impl Display for NativePlanningError { + /// 将错误格式化为可供托管侧诊断的中文文本。 + /// + /// # 参数 + /// - `formatter`: 接收格式化文本的输出缓冲区。 + /// + /// # 返回值 + /// 格式化成功时返回 `Ok(())`，否则返回底层格式化错误。 + fn fmt(&self, formatter: &mut Formatter<'_>) -> std::fmt::Result { + match self { + Self::InvalidInput(message) => write!(formatter, "输入非法：{message}"), + Self::NumericalFailure(message) => write!(formatter, "数值计算失败：{message}"), + Self::NotConverged(message) => write!(formatter, "ICSP 未收敛：{message}"), + } + } +} + +impl std::error::Error for NativePlanningError {} diff --git a/native/flyshot-native-planning/src/ffi.rs b/native/flyshot-native-planning/src/ffi.rs new file mode 100644 index 0000000..4cdaf32 --- /dev/null +++ b/native/flyshot-native-planning/src/ffi.rs @@ -0,0 +1,436 @@ +//! C ABI 边界定义。 + +use std::cell::RefCell; +use std::ffi::{c_char, CString}; +use std::panic::{catch_unwind, AssertUnwindSafe}; +use std::ptr; +use std::slice; + +use crate::error::NativePlanningError; +use crate::icsp::{plan_icsp, IcspInput, IcspOptions}; + +/// 原生调用成功时返回的固定错误码。 +pub const FLYSHOT_SUCCESS: i32 = 0; +/// 输入指针、维度或数值非法时返回的错误码。 +pub const FLYSHOT_ERROR_INVALID_INPUT: i32 = 1; +/// 样条或 ICSP 数值计算无法继续时返回的错误码。 +pub const FLYSHOT_ERROR_NUMERICAL_FAILURE: i32 = 2; +/// 迭代完成但最终 scale 仍超限时返回的错误码。 +pub const FLYSHOT_ERROR_NOT_CONVERGED: i32 = 3; +/// FFI 捕获到未预期 panic 时返回的错误码。 +pub const FLYSHOT_ERROR_INTERNAL: i32 = 4; + +thread_local! { + /// 保存当前线程最近一次原生错误文本，指针仅在下一次原生调用前有效。 + static LAST_ERROR_MESSAGE: RefCell = RefCell::new(CString::default()); +} + +/// 表示 C ABI 输入的 ICSP 迭代选项。 +#[repr(C)] +#[derive(Debug, Clone, Copy)] +pub struct FlyshotIcspOptions { + /// 收敛指标停止阈值。 + pub threshold: f64, + /// 最大迭代索引，必须为非负数。 + pub max_iterations: i32, + /// 非零表示启用最终 scale 检查。 + pub enforce_final_scale: i32, + /// 最终 scale 允许的非负容差。 + pub final_scale_tolerance: f64, +} + +impl Default for FlyshotIcspOptions { + /// 返回与托管 ICSP 默认行为一致的 C ABI 选项。 + /// + /// # 返回值 + /// 可直接传入 `flyshot_icsp_plan` 的默认选项。 + fn default() -> Self { + let options = IcspOptions::default(); + Self { + threshold: options.threshold, + max_iterations: options.max_iterations as i32, + enforce_final_scale: i32::from(options.enforce_final_scale), + final_scale_tolerance: options.final_scale_tolerance, + } + } +} + +/// 表示 Rust 分配并通过 C ABI 返回的 ICSP 结果。 +#[repr(C)] +#[derive(Debug)] +pub struct FlyshotIcspResult { + /// 路点绝对时间连续数组指针。 + pub waypoint_times: *mut f64, + /// 路点时间数组长度。 + pub waypoint_times_len: i32, + /// 分段时长连续数组指针。 + pub segment_durations: *mut f64, + /// 分段时长数组长度。 + pub segment_durations_len: i32, + /// 分段缩放连续数组指针。 + pub segment_scales: *mut f64, + /// 分段缩放数组长度。 + pub segment_scales_len: i32, + /// 历史最优结果对应的迭代轮数。 + pub iterations: i32, + /// 历史最小收敛指标。 + pub threshold: f64, +} + +/// 通过稳定 C ABI 执行 ICSP 规划。 +/// +/// # Safety +/// 调用方必须保证所有非空输入指针在调用期间指向满足声明长度的可读连续数组， +/// `result` 必须指向可写的结果指针槽位；返回的结果只能交给 +/// `flyshot_icsp_result_free` 释放。 +#[no_mangle] +pub unsafe extern "C" fn flyshot_icsp_plan( + waypoints: *const f64, + waypoint_count: i32, + dof: i32, + velocity_limits: *const f64, + acceleration_limits: *const f64, + jerk_limits: *const f64, + options: *const FlyshotIcspOptions, + result: *mut *mut FlyshotIcspResult, +) -> i32 { + clear_last_error(); + + // Debug/测试构建可恢复遗漏 panic；Release panic=abort 下仍依赖边界校验避免 panic。 + match catch_unwind(AssertUnwindSafe(|| unsafe { + flyshot_icsp_plan_impl( + waypoints, + waypoint_count, + dof, + velocity_limits, + acceleration_limits, + jerk_limits, + options, + result, + ) + })) { + Ok(code) => code, + Err(_) => { + set_last_error("Rust ICSP FFI 捕获到未预期的内部 panic"); + FLYSHOT_ERROR_INTERNAL + } + } +} + +/// 释放由 `flyshot_icsp_plan` 返回的结果及其全部连续数组。 +/// +/// # Safety +/// `result` 必须为空指针，或是尚未释放且由本库当前版本分配的结果指针。 +#[no_mangle] +pub unsafe extern "C" fn flyshot_icsp_result_free(result: *mut FlyshotIcspResult) { + if result.is_null() { + return; + } + + // 先收回结果结构所有权，再按原始长度重建 boxed slice 归还数组内存。 + let result = unsafe { Box::from_raw(result) }; + unsafe { + free_f64_array(result.waypoint_times, result.waypoint_times_len); + free_f64_array(result.segment_durations, result.segment_durations_len); + free_f64_array(result.segment_scales, result.segment_scales_len); + } +} + +/// 返回当前线程最近一次原生错误的 UTF-8 C 字符串。 +/// +/// # 返回值 +/// 指针在当前线程下一次原生调用前有效；没有错误时指向空字符串。 +#[no_mangle] +pub extern "C" fn flyshot_last_error_message() -> *const c_char { + LAST_ERROR_MESSAGE.with(|message| message.borrow().as_ptr()) +} + +/// 执行已通过 panic 边界保护的实际 FFI 参数校验和规划调用。 +/// +/// # Safety +/// 参数约束与公开 `flyshot_icsp_plan` 相同，本方法会在解引用前检查空指针和维度。 +#[allow(clippy::too_many_arguments)] +unsafe fn flyshot_icsp_plan_impl( + waypoints: *const f64, + waypoint_count: i32, + dof: i32, + velocity_limits: *const f64, + acceleration_limits: *const f64, + jerk_limits: *const f64, + options: *const FlyshotIcspOptions, + result: *mut *mut FlyshotIcspResult, +) -> i32 { + if result.is_null() { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "result 输出指针不能为空"); + } + unsafe { + *result = ptr::null_mut(); + } + + if waypoints.is_null() { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoints 输入指针不能为空"); + } + if velocity_limits.is_null() || acceleration_limits.is_null() || jerk_limits.is_null() { + return fail( + FLYSHOT_ERROR_INVALID_INPUT, + "velocity/acceleration/jerk 限值指针不能为空", + ); + } + if options.is_null() { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "options 输入指针不能为空"); + } + if waypoint_count < 4 || dof <= 0 { + return fail( + FLYSHOT_ERROR_INVALID_INPUT, + "waypoint_count 必须至少为 4，dof 必须大于零", + ); + } + + let Ok(waypoint_count_usize) = usize::try_from(waypoint_count) else { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoint_count 转换失败"); + }; + let Ok(dof_usize) = usize::try_from(dof) else { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "dof 转换失败"); + }; + let Some(waypoint_value_count) = waypoint_count_usize.checked_mul(dof_usize) else { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "路点数组长度计算溢出"); + }; + let ffi_options = unsafe { *options }; + if ffi_options.max_iterations < 0 { + return fail(FLYSHOT_ERROR_INVALID_INPUT, "max_iterations 不能为负数"); + } + + // 所有原始指针都已完成非空及长度上界校验，此处才建立只读切片。 + let input = IcspInput { + waypoints: unsafe { slice::from_raw_parts(waypoints, waypoint_value_count) }, + waypoint_count: waypoint_count_usize, + dof: dof_usize, + velocity_limits: unsafe { slice::from_raw_parts(velocity_limits, dof_usize) }, + acceleration_limits: unsafe { slice::from_raw_parts(acceleration_limits, dof_usize) }, + jerk_limits: unsafe { slice::from_raw_parts(jerk_limits, dof_usize) }, + options: IcspOptions { + threshold: ffi_options.threshold, + max_iterations: ffi_options.max_iterations as usize, + enforce_final_scale: ffi_options.enforce_final_scale != 0, + final_scale_tolerance: ffi_options.final_scale_tolerance, + }, + }; + + match plan_icsp(&input) { + Ok(plan) => { + let Ok(iterations) = i32::try_from(plan.iterations) else { + return fail(FLYSHOT_ERROR_INTERNAL, "迭代次数超出 C ABI i32 范围"); + }; + let (waypoint_times, waypoint_times_len) = leak_f64_array(plan.waypoint_times); + let (segment_durations, segment_durations_len) = leak_f64_array(plan.segment_durations); + let (segment_scales, segment_scales_len) = leak_f64_array(plan.segment_scales); + let native_result = Box::new(FlyshotIcspResult { + waypoint_times, + waypoint_times_len, + segment_durations, + segment_durations_len, + segment_scales, + segment_scales_len, + iterations, + threshold: plan.threshold, + }); + unsafe { + *result = Box::into_raw(native_result); + } + FLYSHOT_SUCCESS + } + Err(error) => { + let code = error_code(&error); + set_last_error(&error.to_string()); + code + } + } +} + +/// 将 Rust 规划错误映射为稳定 C ABI 错误码。 +/// +/// # 参数 +/// - `error`: 纯算法层返回的错误。 +/// +/// # 返回值 +/// 与错误类别一一对应的固定整数。 +fn error_code(error: &NativePlanningError) -> i32 { + match error { + NativePlanningError::InvalidInput(_) => FLYSHOT_ERROR_INVALID_INPUT, + NativePlanningError::NumericalFailure(_) => FLYSHOT_ERROR_NUMERICAL_FAILURE, + NativePlanningError::NotConverged(_) => FLYSHOT_ERROR_NOT_CONVERGED, + } +} + +/// 写入错误文本并返回指定错误码。 +/// +/// # 参数 +/// - `code`: 需要返回给调用方的稳定错误码。 +/// - `message`: 当前线程可读取的诊断文本。 +/// +/// # 返回值 +/// 原样返回 `code`，便于校验分支直接结束。 +fn fail(code: i32, message: &str) -> i32 { + set_last_error(message); + code +} + +/// 清空当前线程的最近错误文本。 +fn clear_last_error() { + set_last_error(""); +} + +/// 将诊断文本保存到当前线程；内部 NUL 会替换为空格以保持合法 C 字符串。 +/// +/// # 参数 +/// - `message`: 需要保存到线程局部缓冲区的 UTF-8 文本。 +fn set_last_error(message: &str) { + let sanitized = message.replace('\0', " "); + // FFI 路径明确不使用 unwrap；即使替换逻辑未来变化，也保持无 panic 回退。 + #[allow(clippy::manual_unwrap_or_default)] + let c_string = match CString::new(sanitized) { + Ok(value) => value, + Err(_) => CString::default(), + }; + LAST_ERROR_MESSAGE.with(|slot| { + *slot.borrow_mut() = c_string; + }); +} + +/// 将 `Vec` 转为由 C ABI 结果持有的裸指针和 i32 长度。 +/// +/// # 参数 +/// - `values`: 需要转移给结果对象的连续数组。 +/// +/// # 返回值 +/// 数组裸指针及其长度；当前输入长度来源于 i32 路点维度，因此不会溢出。 +fn leak_f64_array(values: Vec) -> (*mut f64, i32) { + let mut values = values.into_boxed_slice(); + let length = i32::try_from(values.len()).unwrap_or(i32::MAX); + let pointer = values.as_mut_ptr(); + std::mem::forget(values); + (pointer, length) +} + +/// 按原始长度重建并释放 Rust 分配的 `f64` boxed slice。 +/// +/// # Safety +/// 指针和长度必须来自 `leak_f64_array` 且尚未释放。 +unsafe fn free_f64_array(pointer: *mut f64, length: i32) { + if pointer.is_null() || length <= 0 { + return; + } + let Ok(length) = usize::try_from(length) else { + return; + }; + let slice_pointer = ptr::slice_from_raw_parts_mut(pointer, length); + drop(unsafe { Box::from_raw(slice_pointer) }); +} + +#[cfg(test)] +mod tests { + use std::ffi::CStr; + use std::ptr; + + use super::{ + flyshot_icsp_plan, flyshot_icsp_result_free, flyshot_last_error_message, + FlyshotIcspOptions, FlyshotIcspResult, FLYSHOT_ERROR_INVALID_INPUT, FLYSHOT_SUCCESS, + }; + + /// 验证空输入指针会返回输入错误，并在线程局部错误文本中保留可诊断信息。 + #[test] + fn ffi_rejects_null_waypoint_pointer_with_error_message() { + let options = FlyshotIcspOptions::default(); + let mut result: *mut FlyshotIcspResult = ptr::null_mut(); + + let error_code = unsafe { + flyshot_icsp_plan( + ptr::null(), + 4, + 1, + [2.0].as_ptr(), + [4.0].as_ptr(), + [8.0].as_ptr(), + &options, + &mut result, + ) + }; + + assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT); + assert!(result.is_null()); + let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) } + .to_string_lossy() + .into_owned(); + assert!(message.contains("waypoints")); + } + + /// 验证正常 C ABI 调用会返回 Rust 分配的结果，并可通过唯一释放函数归还。 + #[test] + fn ffi_returns_and_frees_successful_plan_result() { + let waypoints = [0.0, 0.5, 1.0, 1.5]; + let velocity_limits = [2.0]; + let acceleration_limits = [4.0]; + let jerk_limits = [8.0]; + let options = FlyshotIcspOptions { + enforce_final_scale: 0, + ..FlyshotIcspOptions::default() + }; + let mut result: *mut FlyshotIcspResult = ptr::null_mut(); + + let error_code = unsafe { + flyshot_icsp_plan( + waypoints.as_ptr(), + 4, + 1, + velocity_limits.as_ptr(), + acceleration_limits.as_ptr(), + jerk_limits.as_ptr(), + &options, + &mut result, + ) + }; + + let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) } + .to_string_lossy() + .into_owned(); + assert_eq!(error_code, FLYSHOT_SUCCESS, "{message}"); + assert!(!result.is_null()); + unsafe { + assert_eq!((*result).waypoint_times_len, 4); + assert_eq!((*result).segment_durations_len, 3); + assert_eq!((*result).segment_scales_len, 3); + flyshot_icsp_result_free(result); + } + } + + /// 验证包含非有限数的路点会在 FFI 内转换为输入错误而不是触发 panic。 + #[test] + fn ffi_rejects_non_finite_waypoint_values() { + let waypoints = [0.0, 0.5, f64::NAN, 1.5]; + let velocity_limits = [2.0]; + let acceleration_limits = [4.0]; + let jerk_limits = [8.0]; + let options = FlyshotIcspOptions::default(); + let mut result: *mut FlyshotIcspResult = ptr::null_mut(); + + let error_code = unsafe { + flyshot_icsp_plan( + waypoints.as_ptr(), + 4, + 1, + velocity_limits.as_ptr(), + acceleration_limits.as_ptr(), + jerk_limits.as_ptr(), + &options, + &mut result, + ) + }; + + assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT); + assert!(result.is_null()); + let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) } + .to_string_lossy() + .into_owned(); + assert!(message.contains("有限数")); + } +} diff --git a/native/flyshot-native-planning/src/icsp.rs b/native/flyshot-native-planning/src/icsp.rs new file mode 100644 index 0000000..6a0e2f4 --- /dev/null +++ b/native/flyshot-native-planning/src/icsp.rs @@ -0,0 +1,367 @@ +//! 纯 Rust ICSP 规划算法。 + +use crate::error::NativePlanningError; +use crate::spline::CubicSpline1D; + +/// 描述 ICSP 迭代和最终约束检查选项。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct IcspOptions { + /// 收敛指标 `sum(abs(scale - 1))` 的停止阈值。 + pub threshold: f64, + /// 最大迭代索引；算法与托管实现一致，会执行 `0..=max_iterations`。 + pub max_iterations: usize, + /// 是否在历史最优结果仍有 scale 超限时返回失败。 + pub enforce_final_scale: bool, + /// 最终 scale 检查允许的非负容差。 + pub final_scale_tolerance: f64, +} + +impl Default for IcspOptions { + /// 返回与现有 C# `ICspPlanner` 一致的默认参数。 + /// + /// # 返回值 + /// threshold=1e-3、max_iterations=1000、启用最终 scale 检查且容差为 1e-2。 + fn default() -> Self { + Self { + threshold: 1e-3, + max_iterations: 1000, + enforce_final_scale: true, + final_scale_tolerance: 1e-2, + } + } +} + +/// 描述一次纯 Rust ICSP 调用的扁平输入。 +#[derive(Debug, Clone, Copy)] +pub struct IcspInput<'a> { + /// 按路点行优先排列的连续关节位置数组。 + pub waypoints: &'a [f64], + /// 路点数量。 + pub waypoint_count: usize, + /// 每个路点的关节自由度。 + pub dof: usize, + /// 每个关节的正速度上限。 + pub velocity_limits: &'a [f64], + /// 每个关节的正加速度上限。 + pub acceleration_limits: &'a [f64], + /// 每个关节的正 jerk 上限。 + pub jerk_limits: &'a [f64], + /// 本次规划使用的迭代选项。 + pub options: IcspOptions, +} + +/// 表示 ICSP 历史最优迭代对应的完整规划结果。 +#[derive(Debug, Clone, PartialEq)] +pub struct IcspPlanResult { + /// 每个路点对应的绝对时间，首项固定为零。 + pub waypoint_times: Vec, + /// 历史最优结果的逐段时长。 + pub segment_durations: Vec, + /// 历史最优结果的逐段约束缩放因子。 + pub segment_scales: Vec, + /// 历史最优结果产生时已执行的迭代轮数。 + pub iterations: usize, + /// 历史最小 `sum(abs(scale - 1))`。 + pub threshold: f64, +} + +/// 执行 ICSP 迭代规划并返回历史最优结果。 +/// +/// # 参数 +/// - `input`: 行优先路点、逐关节约束和迭代选项。 +/// +/// # 返回值 +/// 输入合法并通过最终 scale 检查时返回规划结果；否则返回明确错误。 +pub fn plan_icsp(input: &IcspInput<'_>) -> Result { + validate_input(input)?; + + let mut segment_durations = compute_initial_durations(input)?; + let segment_count = segment_durations.len(); + let mut best_result: Option = None; + let mut best_threshold = f64::INFINITY; + + // 迭代范围必须包含 max_iterations 对应轮次，以复刻托管实现的 `<=` 语义。 + for iteration in 0..=input.options.max_iterations { + let waypoint_times = cumulative_times(&segment_durations)?; + let splines = build_joint_splines(input, &waypoint_times)?; + let mut segment_scales = vec![0.0; segment_count]; + + for (segment, segment_scale) in segment_scales.iter_mut().enumerate() { + let mut scale = 0.0_f64; + for (joint, spline) in splines.iter().enumerate() { + let [max_velocity, max_acceleration, max_jerk] = + spline.segment_peak_derivatives(segment)?; + let velocity_scale = (max_velocity / input.velocity_limits[joint]).abs(); + let acceleration_scale = (max_acceleration / input.acceleration_limits[joint]) + .abs() + .sqrt(); + let jerk_scale = (max_jerk / input.jerk_limits[joint]).abs().cbrt(); + scale = scale.max(velocity_scale.max(acceleration_scale.max(jerk_scale))); + } + if !scale.is_finite() { + return Err(NativePlanningError::NumericalFailure(format!( + "段 {segment} 的缩放因子不是有限数" + ))); + } + *segment_scale = scale; + } + + let current_threshold = segment_scales + .iter() + .map(|scale| (scale - 1.0).abs()) + .sum::(); + if !current_threshold.is_finite() { + return Err(NativePlanningError::NumericalFailure( + "ICSP 收敛指标不是有限数".to_owned(), + )); + } + + // 始终保存历史最小指标对应的数据，不能只保留最后一轮。 + if current_threshold < best_threshold { + best_threshold = current_threshold; + best_result = Some(IcspPlanResult { + waypoint_times: waypoint_times.clone(), + segment_durations: segment_durations.clone(), + segment_scales: segment_scales.clone(), + iterations: iteration + 1, + threshold: current_threshold, + }); + } + + if current_threshold < input.options.threshold { + break; + } + + for (duration, scale) in segment_durations.iter_mut().zip(segment_scales.iter()) { + *duration *= *scale; + } + } + + let Some(result) = best_result else { + return Err(NativePlanningError::NumericalFailure( + "ICSP 未产生任何有效迭代结果".to_owned(), + )); + }; + let global_scale = result + .segment_scales + .iter() + .copied() + .fold(0.0_f64, f64::max); + + if input.options.enforce_final_scale && global_scale > 1.0 + input.options.final_scale_tolerance + { + return Err(NativePlanningError::NotConverged(format!( + "global_scale={global_scale:.6} > {:.6}", + 1.0 + input.options.final_scale_tolerance + ))); + } + + Ok(result) +} + +/// 校验 ICSP 输入维度、有限性和正约束。 +/// +/// # 参数 +/// - `input`: 待校验的纯算法输入。 +/// +/// # 返回值 +/// 全部约束满足时返回 `Ok(())`，否则返回输入错误。 +fn validate_input(input: &IcspInput<'_>) -> Result<(), NativePlanningError> { + if input.waypoint_count < 4 { + return Err(NativePlanningError::InvalidInput( + "ICSP 至少需要 4 个示教点".to_owned(), + )); + } + if input.dof == 0 { + return Err(NativePlanningError::InvalidInput( + "自由度必须大于零".to_owned(), + )); + } + let expected_waypoint_values = input + .waypoint_count + .checked_mul(input.dof) + .ok_or_else(|| NativePlanningError::InvalidInput("路点维度乘积溢出".to_owned()))?; + if input.waypoints.len() != expected_waypoint_values { + return Err(NativePlanningError::InvalidInput(format!( + "路点扁平数组长度应为 {expected_waypoint_values}，实际为 {}", + input.waypoints.len() + ))); + } + if input.velocity_limits.len() != input.dof + || input.acceleration_limits.len() != input.dof + || input.jerk_limits.len() != input.dof + { + return Err(NativePlanningError::InvalidInput( + "速度、加速度和 jerk 限值长度必须等于自由度".to_owned(), + )); + } + if input.waypoints.iter().any(|value| !value.is_finite()) { + return Err(NativePlanningError::InvalidInput( + "路点位置必须全部为有限数".to_owned(), + )); + } + if input + .velocity_limits + .iter() + .chain(input.acceleration_limits.iter()) + .chain(input.jerk_limits.iter()) + .any(|limit| !limit.is_finite() || *limit <= 0.0) + { + return Err(NativePlanningError::InvalidInput( + "速度、加速度和 jerk 限值必须全部为有限正数".to_owned(), + )); + } + if !input.options.threshold.is_finite() || input.options.threshold <= 0.0 { + return Err(NativePlanningError::InvalidInput( + "收敛阈值必须为有限正数".to_owned(), + )); + } + if !input.options.final_scale_tolerance.is_finite() || input.options.final_scale_tolerance < 0.0 + { + return Err(NativePlanningError::InvalidInput( + "最终 scale 容差必须为有限非负数".to_owned(), + )); + } + Ok(()) +} + +/// 计算相邻路点在关节空间的欧氏距离，作为初始段时长。 +/// +/// # 参数 +/// - `input`: 已通过维度校验的 ICSP 输入。 +/// +/// # 返回值 +/// 每对相邻路点对应的正段时长。 +fn compute_initial_durations(input: &IcspInput<'_>) -> Result, NativePlanningError> { + let mut durations = Vec::with_capacity(input.waypoint_count - 1); + for segment in 0..input.waypoint_count - 1 { + let start_offset = segment * input.dof; + let end_offset = (segment + 1) * input.dof; + let mut squared_distance = 0.0; + for joint in 0..input.dof { + let difference = + input.waypoints[end_offset + joint] - input.waypoints[start_offset + joint]; + squared_distance += difference * difference; + } + let duration = squared_distance.sqrt(); + if !duration.is_finite() || duration <= 0.0 { + return Err(NativePlanningError::InvalidInput(format!( + "相邻路点段 {segment} 的欧氏距离必须为有限正数" + ))); + } + durations.push(duration); + } + Ok(durations) +} + +/// 将逐段时长累加为首项为零的绝对时间节点。 +/// +/// # 参数 +/// - `segment_durations`: 当前迭代的逐段时长。 +/// +/// # 返回值 +/// 长度比分段时长多一的严格递增时间轴。 +fn cumulative_times(segment_durations: &[f64]) -> Result, NativePlanningError> { + let mut times = Vec::with_capacity(segment_durations.len() + 1); + times.push(0.0); + let mut current = 0.0; + for (segment, duration) in segment_durations.iter().copied().enumerate() { + if !duration.is_finite() || duration <= 0.0 { + return Err(NativePlanningError::NumericalFailure(format!( + "段 {segment} 的迭代时长必须为有限正数" + ))); + } + current += duration; + if !current.is_finite() { + return Err(NativePlanningError::NumericalFailure( + "累计时间轴发生溢出".to_owned(), + )); + } + times.push(current); + } + Ok(times) +} + +/// 为每个关节独立构造 clamped-zero 样条。 +/// +/// # 参数 +/// - `input`: 行优先路点及自由度信息。 +/// - `waypoint_times`: 当前迭代的绝对时间节点。 +/// +/// # 返回值 +/// 按关节顺序排列的单自由度样条集合。 +fn build_joint_splines( + input: &IcspInput<'_>, + waypoint_times: &[f64], +) -> Result, NativePlanningError> { + let mut splines = Vec::with_capacity(input.dof); + for joint in 0..input.dof { + let mut values = Vec::with_capacity(input.waypoint_count); + for waypoint in 0..input.waypoint_count { + values.push(input.waypoints[waypoint * input.dof + joint]); + } + splines.push(CubicSpline1D::new_clamped_zero(waypoint_times, &values)?); + } + Ok(splines) +} + +#[cfg(test)] +mod tests { + use super::{plan_icsp, IcspInput, IcspOptions}; + use crate::error::NativePlanningError; + + /// 验证 ICSP 对基础四路点样本返回完整时间轴、分段结果和有限收敛信息。 + #[test] + fn icsp_returns_complete_result_for_basic_fixture() { + let waypoints = [0.0, 0.0, 1.0, 0.5, 2.0, 1.0, 3.0, 1.5]; + let velocity_limits = [2.0, 2.0]; + let acceleration_limits = [4.0, 4.0]; + let jerk_limits = [8.0, 8.0]; + let input = IcspInput { + waypoints: &waypoints, + waypoint_count: 4, + dof: 2, + velocity_limits: &velocity_limits, + acceleration_limits: &acceleration_limits, + jerk_limits: &jerk_limits, + options: IcspOptions::default(), + }; + + let result = plan_icsp(&input).unwrap(); + + assert_eq!(result.waypoint_times.len(), 4); + assert_eq!(result.segment_durations.len(), 3); + assert_eq!(result.segment_scales.len(), 3); + assert!(result.iterations >= 1); + assert!(result.threshold.is_finite()); + assert!(result + .waypoint_times + .windows(2) + .all(|pair| pair[1] > pair[0])); + } + + /// 验证禁用迭代且最终 scale 明显超限时，纯 Rust 核心返回未收敛错误。 + #[test] + fn icsp_reports_not_converged_when_zero_iterations_cannot_meet_limits() { + let waypoints = [0.0, 1.0, 2.0, 3.0]; + let velocity_limits = [0.01]; + let acceleration_limits = [0.01]; + let jerk_limits = [0.01]; + let input = IcspInput { + waypoints: &waypoints, + waypoint_count: 4, + dof: 1, + velocity_limits: &velocity_limits, + acceleration_limits: &acceleration_limits, + jerk_limits: &jerk_limits, + options: IcspOptions { + max_iterations: 0, + ..IcspOptions::default() + }, + }; + + let error = plan_icsp(&input).unwrap_err(); + + assert!(matches!(error, NativePlanningError::NotConverged(_))); + } +} diff --git a/native/flyshot-native-planning/src/lib.rs b/native/flyshot-native-planning/src/lib.rs new file mode 100644 index 0000000..658ac8d --- /dev/null +++ b/native/flyshot-native-planning/src/lib.rs @@ -0,0 +1,6 @@ +//! Flyshot ICSP 原生规划库的稳定入口。 + +mod error; +mod ffi; +mod icsp; +mod spline; diff --git a/native/flyshot-native-planning/src/spline.rs b/native/flyshot-native-planning/src/spline.rs new file mode 100644 index 0000000..25025b4 --- /dev/null +++ b/native/flyshot-native-planning/src/spline.rs @@ -0,0 +1,277 @@ +//! clamped-zero 三次样条算法。 + +use crate::error::NativePlanningError; + +/// 表示单自由度 clamped-zero 三次样条及其逐段多项式系数。 +#[derive(Debug, Clone)] +pub struct CubicSpline1D { + knots: Vec, + coefficients: Vec<[f64; 4]>, +} + +// 位置和逐阶导数求值是第一阶段要求的原生能力，当前 ICSP 热路径只直接使用解析峰值。 +#[allow(dead_code)] +impl CubicSpline1D { + /// 使用严格递增时间节点和对应值构造首尾速度为零的三次样条。 + /// + /// # 参数 + /// - `times`: 严格递增且全部有限的时间节点，长度至少为 2。 + /// - `values`: 与时间节点一一对应的有限位置值。 + /// + /// # 返回值 + /// 输入合法且三对角方程可解时返回样条，否则返回输入或数值错误。 + pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result { + if times.len() < 2 { + return Err(NativePlanningError::InvalidInput( + "时间节点至少需要 2 个".to_owned(), + )); + } + if values.len() != times.len() { + return Err(NativePlanningError::InvalidInput( + "位置值数量必须与时间节点数量一致".to_owned(), + )); + } + if times.iter().any(|value| !value.is_finite()) + || values.iter().any(|value| !value.is_finite()) + { + return Err(NativePlanningError::InvalidInput( + "时间节点和位置值必须全部为有限数".to_owned(), + )); + } + + let segment_count = times.len() - 1; + let mut durations = Vec::with_capacity(segment_count); + for segment in 0..segment_count { + let duration = times[segment + 1] - times[segment]; + if duration <= 0.0 || !duration.is_finite() { + return Err(NativePlanningError::InvalidInput(format!( + "时间节点必须严格递增，段 {segment} 的时长为 {duration}" + ))); + } + durations.push(duration); + } + + // 与托管实现保持一致：先组装 clamped-zero 边界下的三对角方程右端项。 + let node_count = times.len(); + let mut alpha = vec![0.0; node_count]; + alpha[0] = 6.0 * ((values[1] - values[0]) / durations[0]); + alpha[node_count - 1] = 6.0 + * (0.0 - (values[node_count - 1] - values[node_count - 2]) / durations[node_count - 2]); + for index in 1..node_count - 1 { + alpha[index] = 6.0 + * ((values[index + 1] - values[index]) / durations[index] + - (values[index] - values[index - 1]) / durations[index - 1]); + } + + let mut diagonal = vec![0.0; node_count]; + let mut upper_ratio = vec![0.0; node_count]; + let mut solved_rhs = vec![0.0; node_count]; + diagonal[0] = 2.0 * durations[0]; + upper_ratio[0] = 0.5; + solved_rhs[0] = alpha[0] / diagonal[0]; + + // Thomas 消元逐行处理内部节点，避免建立完整稠密矩阵。 + for index in 1..node_count - 1 { + diagonal[index] = 2.0 * (durations[index - 1] + durations[index]) + - durations[index - 1] * upper_ratio[index - 1]; + if diagonal[index].abs() <= f64::EPSILON || !diagonal[index].is_finite() { + return Err(NativePlanningError::NumericalFailure(format!( + "三对角方程在节点 {index} 出现无效主元" + ))); + } + upper_ratio[index] = durations[index] / diagonal[index]; + solved_rhs[index] = + (alpha[index] - durations[index - 1] * solved_rhs[index - 1]) / diagonal[index]; + } + + diagonal[node_count - 1] = 2.0 * durations[node_count - 2] + - durations[node_count - 2] * upper_ratio[node_count - 2]; + if diagonal[node_count - 1].abs() <= f64::EPSILON || !diagonal[node_count - 1].is_finite() { + return Err(NativePlanningError::NumericalFailure( + "三对角方程末端出现无效主元".to_owned(), + )); + } + solved_rhs[node_count - 1] = (alpha[node_count - 1] + - durations[node_count - 2] * solved_rhs[node_count - 2]) + / diagonal[node_count - 1]; + + // 回代得到各节点二阶导数，再转换为每段局部三次多项式系数。 + let mut second_derivatives = vec![0.0; node_count]; + second_derivatives[node_count - 1] = solved_rhs[node_count - 1]; + for index in (0..node_count - 1).rev() { + second_derivatives[index] = + solved_rhs[index] - upper_ratio[index] * second_derivatives[index + 1]; + } + + let mut coefficients = Vec::with_capacity(segment_count); + for segment in 0..segment_count { + let duration = durations[segment]; + let cubic = + (second_derivatives[segment + 1] - second_derivatives[segment]) / (6.0 * duration); + let quadratic = second_derivatives[segment] / 2.0; + let linear = (values[segment + 1] - values[segment]) / duration + - duration * (2.0 * second_derivatives[segment] + second_derivatives[segment + 1]) + / 6.0; + coefficients.push([cubic, quadratic, linear, values[segment]]); + } + + Ok(Self { + knots: times.to_vec(), + coefficients, + }) + } + + /// 在指定绝对时间求样条位置；超出范围时使用首段或末段外推点。 + /// + /// # 参数 + /// - `time`: 需要求值的绝对时间。 + /// + /// # 返回值 + /// 对应样条位置。 + pub fn position(&self, time: f64) -> f64 { + let (segment, local_time) = self.segment_and_local_time(time); + let [cubic, quadratic, linear, constant] = self.coefficients[segment]; + cubic * local_time * local_time * local_time + + quadratic * local_time * local_time + + linear * local_time + + constant + } + + /// 在指定绝对时间求一阶导数。 + /// + /// # 参数 + /// - `time`: 需要求值的绝对时间。 + /// + /// # 返回值 + /// 对应样条速度。 + pub fn velocity(&self, time: f64) -> f64 { + let (segment, local_time) = self.segment_and_local_time(time); + let [cubic, quadratic, linear, _] = self.coefficients[segment]; + 3.0 * cubic * local_time * local_time + 2.0 * quadratic * local_time + linear + } + + /// 在指定绝对时间求二阶导数。 + /// + /// # 参数 + /// - `time`: 需要求值的绝对时间。 + /// + /// # 返回值 + /// 对应样条加速度。 + pub fn acceleration(&self, time: f64) -> f64 { + let (segment, local_time) = self.segment_and_local_time(time); + let [cubic, quadratic, _, _] = self.coefficients[segment]; + 6.0 * cubic * local_time + 2.0 * quadratic + } + + /// 在指定绝对时间求三阶导数。 + /// + /// # 参数 + /// - `time`: 需要求值的绝对时间。 + /// + /// # 返回值 + /// 对应样条 jerk。 + pub fn jerk(&self, time: f64) -> f64 { + let (segment, _) = self.segment_and_local_time(time); + 6.0 * self.coefficients[segment][0] + } + + /// 解析计算指定段的一至三阶导数绝对值峰值。 + /// + /// # 参数 + /// - `segment_index`: 从零开始的段索引。 + /// + /// # 返回值 + /// `[速度峰值, 加速度峰值, jerk 峰值]`；索引越界时返回输入错误。 + pub fn segment_peak_derivatives( + &self, + segment_index: usize, + ) -> Result<[f64; 3], NativePlanningError> { + let Some([cubic, quadratic, linear, _]) = self.coefficients.get(segment_index).copied() + else { + return Err(NativePlanningError::InvalidInput(format!( + "样条段索引 {segment_index} 越界" + ))); + }; + let duration = self.knots[segment_index + 1] - self.knots[segment_index]; + + // 一阶导为二次函数，绝对值峰值只可能出现在端点或段内顶点。 + let mut velocity_peak = linear + .abs() + .max((3.0 * cubic * duration * duration + 2.0 * quadratic * duration + linear).abs()); + if cubic.abs() > 1e-12 { + let critical = -quadratic / (3.0 * cubic); + if critical > 0.0 && critical < duration { + velocity_peak = velocity_peak.max( + (3.0 * cubic * critical * critical + 2.0 * quadratic * critical + linear).abs(), + ); + } + } + + let acceleration_peak = (2.0 * quadratic) + .abs() + .max((6.0 * cubic * duration + 2.0 * quadratic).abs()); + let jerk_peak = (6.0 * cubic).abs(); + Ok([velocity_peak, acceleration_peak, jerk_peak]) + } + + /// 定位时间所在样条段并计算相对段起点的局部时间。 + /// + /// # 参数 + /// - `time`: 需要定位的绝对时间。 + /// + /// # 返回值 + /// `(段索引, 局部时间)`；时间超界时钳制到首尾节点。 + fn segment_and_local_time(&self, time: f64) -> (usize, f64) { + let last_segment = self.coefficients.len() - 1; + if time <= self.knots[0] { + return (0, 0.0); + } + + let last_knot = self.knots[self.knots.len() - 1]; + if time >= last_knot { + return ( + last_segment, + self.knots[last_segment + 1] - self.knots[last_segment], + ); + } + + let insertion = self.knots.partition_point(|knot| *knot <= time); + let segment = insertion.saturating_sub(1).min(last_segment); + (segment, time - self.knots[segment]) + } +} + +#[cfg(test)] +mod tests { + use super::CubicSpline1D; + + /// 验证 clamped-zero 样条严格命中首尾路点，并满足两端零速度边界。 + #[test] + fn clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity() { + let times = [0.0, 1.0, 2.0, 3.0]; + let values = [0.0, 1.0, 0.5, 2.0]; + let spline = CubicSpline1D::new_clamped_zero(×, &values).unwrap(); + + assert!((spline.position(0.0) - 0.0).abs() <= 1e-12); + assert!((spline.position(3.0) - 2.0).abs() <= 1e-12); + assert!(spline.velocity(0.0).abs() <= 1e-12); + assert!(spline.velocity(3.0).abs() <= 1e-12); + assert!(spline.acceleration(1.5).is_finite()); + assert!(spline.jerk(1.5).is_finite()); + } + + /// 验证每段速度、加速度和 jerk 的解析峰值不会漏掉段内速度极值。 + #[test] + fn segment_peak_derivatives_include_internal_velocity_extremum() { + let times = [0.0, 1.0, 2.0, 3.0]; + let values = [0.0, 1.0, 0.5, 2.0]; + let spline = CubicSpline1D::new_clamped_zero(×, &values).unwrap(); + + let peaks = spline.segment_peak_derivatives(0).unwrap(); + + assert!(peaks[0] >= spline.velocity(0.0).abs()); + assert!(peaks[0] >= spline.velocity(1.0).abs()); + assert!(peaks[1].is_finite() && peaks[1] > 0.0); + assert!(peaks[2].is_finite() && peaks[2] > 0.0); + } +} diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index a4d9b56..3837eef 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -3,6 +3,7 @@ using Flyshot.Core.Domain; using Flyshot.Core.Planning; using Flyshot.Core.Planning.Sampling; using Flyshot.Core.Triggering; +using Flyshot.Native.Planning; using Microsoft.Extensions.Logging; namespace Flyshot.ControllerClientCompat; @@ -29,6 +30,7 @@ public sealed class ControllerClientTrajectoryOrchestrator private const double SmoothStartStopBlend = 0.60; private readonly ICspPlanner _icspPlanner; + private readonly RustIcspPlanner _rustIcspPlanner; private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner; private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver()); private readonly Dictionary _flyshotCache = new(StringComparer.Ordinal); @@ -45,6 +47,7 @@ public sealed class ControllerClientTrajectoryOrchestrator { _logger = logger; _icspPlanner = new(logger: loggerFactory?.CreateLogger()); + _rustIcspPlanner = new(); _selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger()); } @@ -209,6 +212,7 @@ public sealed class ControllerClientTrajectoryOrchestrator return normalized switch { "icsp" => PlanningMethod.Icsp, + "icsp_rust" => PlanningMethod.IcspRust, "doubles" => PlanningMethod.Doubles, _ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method)) }; @@ -225,6 +229,7 @@ public sealed class ControllerClientTrajectoryOrchestrator return normalized switch { "icsp" => PlanningMethod.Icsp, + "icsp_rust" => PlanningMethod.IcspRust, "self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp, "doubles" => PlanningMethod.Doubles, _ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method)) @@ -242,6 +247,7 @@ public sealed class ControllerClientTrajectoryOrchestrator return method switch { PlanningMethod.Icsp => _icspPlanner.Plan(request), + PlanningMethod.IcspRust => _rustIcspPlanner.Plan(request), PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5), PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"), _ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。") diff --git a/src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj b/src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj index ba5ff6c..b925f9e 100644 --- a/src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj +++ b/src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj @@ -5,6 +5,7 @@ + diff --git a/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs index 67aa6b7..292eda8 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs @@ -9,7 +9,7 @@ public sealed class FlyshotExecutionOptions /// 初始化飞拍轨迹执行参数。 /// /// 执行前是否自动移动到轨迹起点。 - /// 轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。 + /// 轨迹生成方法，支持 `icsp`、`icsp_rust`、`doubles` 或 `self-adapt-icsp`。 /// 是否保存轨迹信息。 /// 是否优先复用已规划轨迹缓存。 /// 是否等待机器人执行完整条飞拍轨迹后再返回。 diff --git a/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs b/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs index eb02bcd..f666043 100644 --- a/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs +++ b/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs @@ -8,7 +8,7 @@ public sealed class TrajectoryExecutionOptions /// /// 初始化普通轨迹执行参数。 /// - /// 轨迹生成方法，支持 `icsp` 或 `doubles`。 + /// 轨迹生成方法，支持 `icsp`、`icsp_rust` 或 `doubles`。 /// 是否保存轨迹信息。 public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false) { diff --git a/src/Flyshot.Core.Domain/TrajectoryRequest.cs b/src/Flyshot.Core.Domain/TrajectoryRequest.cs index ece4d1e..b91bae8 100644 --- a/src/Flyshot.Core.Domain/TrajectoryRequest.cs +++ b/src/Flyshot.Core.Domain/TrajectoryRequest.cs @@ -74,6 +74,11 @@ public enum PlanningMethod /// Icsp, + /// + /// 使用 Rust 原生库实现的 ICSP 规划器。 + /// + IcspRust, + /// /// Uses the adaptive ICSP planner that may insert additional points. /// diff --git a/src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj b/src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj new file mode 100644 index 0000000..d9dd29c --- /dev/null +++ b/src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj @@ -0,0 +1,15 @@ + + + + net8.0 + enable + enable + true + + + + + + + + diff --git a/src/Flyshot.Native.Planning/NativeIcspInteropModels.cs b/src/Flyshot.Native.Planning/NativeIcspInteropModels.cs new file mode 100644 index 0000000..f2ff823 --- /dev/null +++ b/src/Flyshot.Native.Planning/NativeIcspInteropModels.cs @@ -0,0 +1,77 @@ +using System.Runtime.InteropServices; + +namespace Flyshot.Native.Planning; + +/// +/// 与 Rust FlyshotIcspOptions 保持逐字段一致的 C ABI 选项结构。 +/// +[StructLayout(LayoutKind.Sequential)] +internal struct NativeIcspOptions +{ + /// + /// 收敛指标停止阈值。 + /// + internal double Threshold; + + /// + /// 最大迭代索引。 + /// + internal int MaxIterations; + + /// + /// 非零表示启用最终 scale 检查。 + /// + internal int EnforceFinalScale; + + /// + /// 最终 scale 允许的非负容差。 + /// + internal double FinalScaleTolerance; +} + +/// +/// 与 Rust FlyshotIcspResult 保持逐字段一致的 C ABI 结果结构。 +/// +[StructLayout(LayoutKind.Sequential)] +internal struct NativeIcspResult +{ + /// + /// 路点绝对时间连续数组指针。 + /// + internal nint WaypointTimes; + + /// + /// 路点绝对时间数组长度。 + /// + internal int WaypointTimesLength; + + /// + /// 分段时长连续数组指针。 + /// + internal nint SegmentDurations; + + /// + /// 分段时长数组长度。 + /// + internal int SegmentDurationsLength; + + /// + /// 分段缩放连续数组指针。 + /// + internal nint SegmentScales; + + /// + /// 分段缩放数组长度。 + /// + internal int SegmentScalesLength; + + /// + /// 历史最优结果对应的迭代轮数。 + /// + internal int Iterations; + + /// + /// 历史最小收敛指标。 + /// + internal double Threshold; +} diff --git a/src/Flyshot.Native.Planning/NativeIcspMethods.cs b/src/Flyshot.Native.Planning/NativeIcspMethods.cs new file mode 100644 index 0000000..a15c9e8 --- /dev/null +++ b/src/Flyshot.Native.Planning/NativeIcspMethods.cs @@ -0,0 +1,130 @@ +using System.Runtime.InteropServices; + +namespace Flyshot.Native.Planning; + +/// +/// 声明 Rust 原生规划库导出的稳定 C ABI 方法。 +/// +internal static partial class NativeIcspMethods +{ + /// + /// 保护原生库句柄首次初始化，避免并发规划重复加载。 + /// + private static readonly object LibraryLoadLock = new(); + + /// + /// 保存已加载原生库句柄，使 P/Invoke 生命周期覆盖整个进程。 + /// + private static nint _libraryHandle; + + /// + /// 保存当前进程已绑定到 P/Invoke 逻辑名称的实际库路径。 + /// + private static string? _loadedLibraryPath; + + /// + /// 初始化程序集级 DllImport 解析器，把逻辑库名绑定到显式加载句柄。 + /// + static NativeIcspMethods() + { + NativeLibrary.SetDllImportResolver( + typeof(NativeIcspMethods).Assembly, + ResolveImportedLibrary); + } + + /// + /// 使用指定解析器确保原生规划库已加载。 + /// + /// 负责生成平台路径并加载原生库的解析器。 + 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; + } + } + + /// + /// 调用 Rust ICSP 入口并返回错误码和结果指针。 + /// + /// 行优先连续路点数组。 + /// 路点数量。 + /// 每个路点的关节自由度。 + /// 逐关节速度上限。 + /// 逐关节加速度上限。 + /// 逐关节 jerk 上限。 + /// 与 Rust C ABI 对齐的规划选项。 + /// 接收 Rust 分配的结果指针。 + /// 零表示成功，非零表示固定类别的原生错误。 + [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); + + /// + /// 释放由 Rust 分配的 ICSP 结果对象。 + /// + /// Rust 返回的结果对象指针；允许为空指针。 + [LibraryImport( + NativePlanningLibraryResolver.LogicalLibraryName, + EntryPoint = "flyshot_icsp_result_free")] + internal static partial void FlyshotIcspResultFree(nint result); + + /// + /// 获取当前线程最近一次 Rust 调用保存的 UTF-8 错误文本。 + /// + /// 指向线程局部 C 字符串的只读指针。 + [LibraryImport( + NativePlanningLibraryResolver.LogicalLibraryName, + EntryPoint = "flyshot_last_error_message")] + internal static partial nint FlyshotLastErrorMessage(); + + /// + /// 将固定逻辑库名解析为已经显式加载的系统句柄。 + /// + /// P/Invoke 请求的逻辑库名。 + /// 发起请求的托管程序集。 + /// 调用方提供的可选搜索路径。 + /// 逻辑名称匹配时返回已加载句柄，否则返回零交给系统继续解析。 + 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; + } +} diff --git a/src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs b/src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs new file mode 100644 index 0000000..9bd8c49 --- /dev/null +++ b/src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs @@ -0,0 +1,24 @@ +namespace Flyshot.Native.Planning; + +/// +/// 描述 Rust ICSP 规划器使用的收敛和最终约束选项。 +/// +/// 收敛指标 sum(abs(scale - 1)) 的停止阈值。 +/// 最大迭代索引；执行范围包含该索引对应轮次。 +/// 是否拒绝历史最优结果中仍然超限的段。 +/// 最终段缩放因子相对 1.0 的非负容差。 +public sealed record NativeIcspPlannerOptions( + double Threshold, + int MaxIterations, + bool EnforceFinalScale, + double FinalScaleTolerance) +{ + /// + /// 获取与现有托管 ICspPlanner 一致的默认选项。 + /// + public static NativeIcspPlannerOptions Default { get; } = new( + Threshold: 1e-3, + MaxIterations: 1000, + EnforceFinalScale: true, + FinalScaleTolerance: 1e-2); +} diff --git a/src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs b/src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs new file mode 100644 index 0000000..fb4be20 --- /dev/null +++ b/src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs @@ -0,0 +1,37 @@ +using Microsoft.Win32.SafeHandles; + +namespace Flyshot.Native.Planning; + +/// +/// 封装 Rust 分配的 ICSP 结果对象，确保托管异常路径也会调用原生释放函数。 +/// +internal sealed class NativeIcspSafeHandle : SafeHandleZeroOrMinusOneIsInvalid +{ + /// + /// 初始化一个尚未持有原生结果的安全句柄。 + /// + public NativeIcspSafeHandle() + : base(ownsHandle: true) + { + } + + /// + /// 初始化并接管 Rust 返回的非零结果指针。 + /// + /// 由 flyshot_icsp_plan 分配的结果指针。 + internal NativeIcspSafeHandle(nint result) + : this() + { + SetHandle(result); + } + + /// + /// 调用 Rust 导出的释放函数归还结果及其连续数组内存。 + /// + /// 释放函数调用完成后返回 。 + protected override bool ReleaseHandle() + { + NativeIcspMethods.FlyshotIcspResultFree(handle); + return true; + } +} diff --git a/src/Flyshot.Native.Planning/NativePlanningException.cs b/src/Flyshot.Native.Planning/NativePlanningException.cs new file mode 100644 index 0000000..a65665b --- /dev/null +++ b/src/Flyshot.Native.Planning/NativePlanningException.cs @@ -0,0 +1,24 @@ +namespace Flyshot.Native.Planning; + +/// +/// 表示原生规划库加载、调用或数值计算失败。 +/// +public sealed class NativePlanningException : Exception +{ + /// + /// 初始化包含原生错误码和底层异常的规划异常。 + /// + /// 供调用方诊断的错误说明。 + /// Rust C ABI 返回的错误码；托管加载错误使用负数。 + /// 触发本异常的底层异常；没有时为 。 + public NativePlanningException(string message, int errorCode, Exception? innerException = null) + : base(message, innerException) + { + ErrorCodeValue = errorCode; + } + + /// + /// 获取 Rust C ABI 错误码或托管互操作错误码。 + /// + public int ErrorCodeValue { get; } +} diff --git a/src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs b/src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs new file mode 100644 index 0000000..f03ec96 --- /dev/null +++ b/src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs @@ -0,0 +1,104 @@ +using System.Runtime.InteropServices; + +namespace Flyshot.Native.Planning; + +/// +/// 描述当前进程加载原生规划库时使用的平台信息。 +/// +/// 当前操作系统名称。 +/// 当前进程体系结构。 +public readonly record struct NativePlanningPlatform(string OperatingSystem, Architecture Architecture); + +/// +/// 按当前操作系统和体系结构解析并加载 Rust 原生规划库。 +/// +public sealed class NativePlanningLibraryResolver +{ + /// + /// 原生规划库的逻辑名称。 + /// + public const string LogicalLibraryName = "flyshot_native_planning"; + + private readonly Func _resolveLibraryPath; + + /// + /// 初始化使用默认部署目录解析规则的原生库解析器。 + /// + public NativePlanningLibraryResolver() + : this(ResolveDefaultLibraryPath) + { + } + + /// + /// 初始化使用自定义路径解析规则的原生库解析器。 + /// + /// 根据当前平台返回原生库路径或名称的函数。 + public NativePlanningLibraryResolver(Func resolveLibraryPath) + { + _resolveLibraryPath = resolveLibraryPath ?? throw new ArgumentNullException(nameof(resolveLibraryPath)); + } + + /// + /// 解析当前平台应加载的原生库路径。 + /// + /// 原生库路径或可被系统加载器识别的库名称。 + public string ResolveLibraryPath() + { + string operatingSystem = OperatingSystem.IsWindows() + ? "Windows" + : OperatingSystem.IsLinux() + ? "Linux" + : "Unsupported"; + return _resolveLibraryPath(new NativePlanningPlatform(operatingSystem, RuntimeInformation.ProcessArchitecture)); + } + + /// + /// 加载当前平台的原生规划库并返回系统句柄。 + /// + /// 由 管理的非零库句柄。 + /// 库缺失、架构不匹配或系统不受支持时抛出。 + 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); + } + } + + /// + /// 根据当前平台生成与托管程序集同目录的默认原生库路径。 + /// + /// 当前操作系统和进程体系结构。 + /// 默认原生库绝对路径。 + 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); + } +} diff --git a/src/Flyshot.Native.Planning/RustIcspPlanner.cs b/src/Flyshot.Native.Planning/RustIcspPlanner.cs new file mode 100644 index 0000000..c30b1fa --- /dev/null +++ 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; + +/// +/// 通过稳定 C ABI 调用 Rust ICSP 核心，并把结果转换为现有托管轨迹模型。 +/// +public sealed class RustIcspPlanner +{ + private readonly NativeIcspPlannerOptions _options; + private readonly NativePlanningLibraryResolver _libraryResolver; + + /// + /// 初始化 Rust ICSP 规划器。 + /// + /// 与托管 ICSP 对齐的收敛及最终约束选项。 + /// 负责解析 Windows/Linux x64 原生库的加载器。 + public RustIcspPlanner( + NativeIcspPlannerOptions? options = null, + NativePlanningLibraryResolver? libraryResolver = null) + { + _options = options ?? NativeIcspPlannerOptions.Default; + _libraryResolver = libraryResolver ?? new NativePlanningLibraryResolver(); + ValidateOptions(_options); + } + + /// + /// 使用 Rust 原生核心规划给定关节路点。 + /// + /// 包含机器人约束和示教路点的规划请求。 + /// 与现有托管规划链路兼容的轨迹结果。 + /// 原生库加载或规划调用失败时抛出。 + 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(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); + } + } + + /// + /// 校验托管规划选项，防止非法数值穿过 C ABI。 + /// + /// 待校验的原生规划选项。 + 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 容差必须为有限非负数。"); + } + } + + /// + /// 校验规划请求与机器人自由度，确保扁平数组布局可安全建立。 + /// + /// 待传入 Rust 的规划请求。 + 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)); + } + } + + /// + /// 将托管路点按行优先顺序复制为连续数组。 + /// + /// 包含路点的规划请求。 + /// 已校验的路点数量。 + /// 已校验的机器人自由度。 + /// 长度为路点数量乘自由度的连续数组。 + 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; + } + + /// + /// 将 Rust 连续数组复制到独立托管数组，避免结果释放后继续引用原生内存。 + /// + /// Rust 结果中的连续数组指针。 + /// 数组元素数量。 + /// 用于异常诊断的 ABI 字段名。 + /// 与原生结果生命周期解耦的托管数组。 + 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; + } + + /// + /// 读取 Rust 当前线程保存的最近错误文本。 + /// + /// UTF-8 错误文本；空指针时返回占位诊断。 + private static string ReadLastErrorMessage() + { + nint messagePointer = NativeIcspMethods.FlyshotLastErrorMessage(); + return messagePointer == nint.Zero + ? "原生库未提供错误文本" + : Marshal.PtrToStringUTF8(messagePointer) ?? "原生错误文本不是有效 UTF-8"; + } +} diff --git a/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj b/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj index 06712ec..99129ec 100644 --- a/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj +++ b/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj @@ -28,6 +28,7 @@ + diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index b5e35f3..aaba0b0 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -106,6 +106,113 @@ public sealed class RuntimeOrchestrationTests Assert.Single(bundle.Result.TriggerTimeline); } + /// + /// 验证普通轨迹使用准确字串 icsp_rust 时会真实调用 Rust DLL，并在结果中标记原生后端。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesRustPlanner() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + + var bundle = orchestrator.PlanOrdinaryTrajectory( + robot, + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.001, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.002, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.003, 0.0, 0.0, 0.0, 0.0, 0.0] + ], + new TrajectoryExecutionOptions(method: "icsp_rust")); + + Assert.Equal(PlanningMethod.IcspRust, bundle.PlannedTrajectory.Method); + Assert.Equal(PlanningMethod.IcspRust, bundle.Result.Method); + } + + /// + /// 验证飞拍轨迹使用准确字串 icsp_rust 时会真实调用 Rust DLL，同时保留触发时间轴。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRustPlanner() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + + var bundle = orchestrator.PlanUploadedFlyshot( + robot, + uploaded, + new FlyshotExecutionOptions(method: "icsp_rust", useCache: false)); + + Assert.Equal(PlanningMethod.IcspRust, bundle.PlannedTrajectory.Method); + Assert.Equal(PlanningMethod.IcspRust, bundle.Result.Method); + Assert.Single(bundle.Result.ShotEvents); + Assert.Single(bundle.Result.TriggerTimeline); + } + + /// + /// 验证普通轨迹默认 icsp 仍使用现有 C# 规划器。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_KeepsManagedIcspPlanner() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + + var bundle = orchestrator.PlanOrdinaryTrajectory( + robot, + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.001, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.002, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.003, 0.0, 0.0, 0.0, 0.0, 0.0] + ], + new TrajectoryExecutionOptions(method: "icsp")); + + Assert.Equal(PlanningMethod.Icsp, bundle.PlannedTrajectory.Method); + Assert.Equal(PlanningMethod.Icsp, bundle.Result.Method); + } + + /// + /// 验证 self-adapt-icsp 仍使用 C# 自适应规划器，不切换到 Rust。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_KeepsManagedSelfAdaptPlanner() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = new RobotProfile( + name: "ManagedSelfAdaptTestRobot", + modelPath: "Models/ManagedSelfAdaptTest.robot", + degreesOfFreedom: 6, + jointLimits: Enumerable.Range(1, 6) + .Select(static index => new JointLimit($"J{index}", 1.0, 2.0, 10.0)) + .ToArray(), + jointCouplings: [], + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + var uploaded = new ControllerClientCompatUploadedTrajectory( + name: "managed-self-adapt", + waypoints: + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.4, 0.1, 0.0, 0.0, 0.0, 0.0], + [0.8, 0.3, 0.0, 0.0, 0.0, 0.0], + [1.0, 0.2, 0.0, 0.0, 0.0, 0.0] + ], + shotFlags: [false, true, false, false], + offsetValues: [0, 0, 0, 0], + addressGroups: [[], [7], [], []]); + + var bundle = orchestrator.PlanUploadedFlyshot( + robot, + uploaded, + new FlyshotExecutionOptions(method: "self-adapt-icsp", useCache: false)); + + Assert.Equal(PlanningMethod.SelfAdaptIcsp, bundle.PlannedTrajectory.Method); + Assert.Equal(PlanningMethod.SelfAdaptIcsp, bundle.Result.Method); + Assert.Single(bundle.Result.TriggerTimeline); + } + /// /// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。 /// diff --git a/tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs b/tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs new file mode 100644 index 0000000..0fdd17f --- /dev/null +++ b/tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs @@ -0,0 +1,135 @@ +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Native.Planning; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 Rust ICSP 托管互操作边界，包括原生库加载失败时的诊断信息。 +/// +public sealed class RustIcspPlannerInteropTests +{ + /// + /// 验证显式指定不存在的原生库时，规划器会抛出包含库路径的明确异常。 + /// + [Fact] + public void RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing() + { + var planner = new RustIcspPlanner( + options: NativeIcspPlannerOptions.Default, + libraryResolver: new NativePlanningLibraryResolver(_ => "missing-library")); + + var exception = Assert.Throws(() => planner.Plan(CreateSimpleRequest())); + + Assert.Contains("missing-library", exception.Message); + } + + /// + /// 验证实际原生规划结果会被复制为现有 。 + /// + [Fact] + public void RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult() + { + var planner = new RustIcspPlanner( + NativeIcspPlannerOptions.Default with { EnforceFinalScale = false }); + + var result = planner.Plan(CreateSimpleRequest()); + + Assert.Equal(PlanningMethod.IcspRust, result.Method); + Assert.Equal(result.PlannedWaypointCount, result.WaypointTimes.Count); + Assert.Equal(result.PlannedWaypointCount - 1, result.SegmentDurations.Count); + Assert.Equal(result.SegmentDurations.Count, result.SegmentScales.Count); + } + + /// + /// 验证互操作测试夹具在两套实现中都由相同的最终 scale 规则拒绝。 + /// + [Fact] + public void ManagedIcspPlanner_RejectsInteropFixtureWithExpectedFinalScale() + { + var planner = new ICspPlanner(); + + var exception = Assert.Throws( + () => planner.Plan(CreateSimpleRequest())); + + Assert.Contains("global_scale=1.058684", exception.Message); + } + + /// + /// 验证进程已加载默认 DLL 后，显式指定其他缺失库仍会暴露部署错误。 + /// + [Fact] + public void RustIcspPlanner_DoesNotReuseLoadedLibrary_ForDifferentExplicitPath() + { + var defaultPlanner = new RustIcspPlanner( + NativeIcspPlannerOptions.Default with { EnforceFinalScale = false }); + _ = defaultPlanner.Plan(CreateSimpleRequest()); + var missingPlanner = new RustIcspPlanner( + options: NativeIcspPlannerOptions.Default, + libraryResolver: new NativePlanningLibraryResolver(_ => "different-missing-library")); + + var exception = Assert.Throws( + () => missingPlanner.Plan(CreateSimpleRequest())); + + Assert.Contains("different-missing-library", exception.Message); + } + + /// + /// 验证 Rust 未收敛错误码和线程局部错误文本会转换为可诊断的托管异常。 + /// + [Fact] + public void RustIcspPlanner_ConvertsNativeNotConvergedError() + { + var planner = new RustIcspPlanner( + NativeIcspPlannerOptions.Default with { MaxIterations = 0 }); + + var exception = Assert.Throws( + () => planner.Plan(CreateSimpleRequest())); + + Assert.Equal(3, exception.ErrorCodeValue); + Assert.Contains("global_scale", exception.Message); + } + + /// + /// 创建用于互操作边界测试的最小四路点单自由度规划请求。 + /// + /// 满足 ICSP 最小路点数量要求的轨迹请求。 + private static TrajectoryRequest CreateSimpleRequest() + { + var robot = new RobotProfile( + name: "rust-interop-test", + modelPath: "test.robot", + degreesOfFreedom: 1, + jointLimits: + [ + new JointLimit( + jointName: "J1", + velocityLimit: 2.0, + accelerationLimit: 4.0, + jerkLimit: 8.0) + ], + jointCouplings: [], + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + var program = new FlyshotProgram( + name: "rust-interop-test", + waypoints: + [ + new JointWaypoint([0.0]), + new JointWaypoint([0.5]), + new JointWaypoint([1.0]), + new JointWaypoint([1.5]) + ], + shotFlags: [false, false, false, false], + offsetValues: [0, 0, 0, 0], + addressGroups: + [ + new IoAddressGroup([]), + new IoAddressGroup([]), + new IoAddressGroup([]), + new IoAddressGroup([]) + ]); + + return new TrajectoryRequest(robot, program, PlanningMethod.Icsp); + } +} diff --git a/tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs b/tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs new file mode 100644 index 0000000..13c42e4 --- /dev/null +++ b/tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs @@ -0,0 +1,160 @@ +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Native.Planning; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 Rust ICSP 与现有 C# 基准实现保持逐项数值一致。 +/// +public sealed class RustIcspPlannerParityTests +{ + /// + /// 验证基础六自由度兼容样本的时间轴、段时长、段缩放和迭代信息完全对齐。 + /// + [Fact] + public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture() + { + TrajectoryRequest request = CreateCompatibilityRequest(); + PlannedTrajectory managed = new ICspPlanner().Plan(request); + PlannedTrajectory native = new RustIcspPlanner().Plan(request); + + Assert.Equal(PlanningMethod.Icsp, managed.Method); + Assert.Equal(PlanningMethod.IcspRust, native.Method); + AssertTrajectoryMatches(managed, native, tolerance: 1e-9); + } + + /// + /// 验证关闭最终 scale 拒绝后，多自由度非均匀路点仍与托管算法逐项对齐。 + /// + [Fact] + public void RustIcspPlanner_MatchesManagedIcspPlanner_ForNonUniformFixture() + { + TrajectoryRequest request = CreateNonUniformRequest(); + var options = NativeIcspPlannerOptions.Default with { EnforceFinalScale = false }; + PlannedTrajectory managed = new ICspPlanner(enforceFinalScale: false).Plan(request); + PlannedTrajectory native = new RustIcspPlanner(options).Plan(request); + + Assert.Equal(PlanningMethod.Icsp, managed.Method); + Assert.Equal(PlanningMethod.IcspRust, native.Method); + AssertTrajectoryMatches(managed, native, tolerance: 1e-9); + } + + /// + /// 创建与现有 PlanningCompatibilityTests 基础用例一致的六自由度请求。 + /// + /// 可由默认 ICSP 配置收敛的稳定规划请求。 + private static TrajectoryRequest CreateCompatibilityRequest() + { + return CreateRequest( + velocityLimits: [1, 1, 1, 1, 1, 1], + accelerationLimits: [2, 2, 2, 2, 2, 2], + jerkLimits: [10, 10, 10, 10, 10, 10], + waypointRows: + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.4, 0.1, 0.0, 0.0, 0.0, 0.0], + [0.8, 0.3, 0.0, 0.0, 0.0, 0.0], + [1.0, 0.2, 0.0, 0.0, 0.0, 0.0] + ]); + } + + /// + /// 创建用于覆盖多自由度、非均匀段距离和不同约束的规划请求。 + /// + /// 关闭最终 scale 拒绝后可用于数值对拍的稳定请求。 + private static TrajectoryRequest CreateNonUniformRequest() + { + return CreateRequest( + velocityLimits: [1.2, 0.9, 1.5], + accelerationLimits: [2.5, 1.8, 3.0], + jerkLimits: [12.0, 9.0, 15.0], + waypointRows: + [ + [0.0, 0.0, 0.0], + [0.2, -0.1, 0.4], + [0.9, 0.3, 0.7], + [1.1, 0.8, 0.2], + [1.6, 0.4, -0.1] + ]); + } + + /// + /// 根据逐关节限制和路点矩阵创建完整领域请求。 + /// + /// 逐关节速度上限。 + /// 逐关节加速度上限。 + /// 逐关节 jerk 上限。 + /// 按路点排列的关节位置矩阵。 + /// 可同时传给 C# 与 Rust 规划器的请求。 + private static TrajectoryRequest CreateRequest( + IReadOnlyList velocityLimits, + IReadOnlyList accelerationLimits, + IReadOnlyList jerkLimits, + IReadOnlyList waypointRows) + { + var robot = new RobotProfile( + name: "RustParityRobot", + modelPath: "Models/RustParity.robot", + degreesOfFreedom: velocityLimits.Count, + jointLimits: velocityLimits + .Select((velocity, index) => new JointLimit( + $"J{index + 1}", + velocity, + accelerationLimits[index], + jerkLimits[index])) + .ToArray(), + jointCouplings: [], + servoPeriod: TimeSpan.FromMilliseconds(8), + triggerPeriod: TimeSpan.FromMilliseconds(8)); + var program = new FlyshotProgram( + name: "rust-parity", + waypoints: waypointRows.Select(row => new JointWaypoint(row)).ToArray(), + shotFlags: Enumerable.Repeat(false, waypointRows.Count).ToArray(), + offsetValues: Enumerable.Repeat(0, waypointRows.Count).ToArray(), + addressGroups: Enumerable.Range(0, waypointRows.Count) + .Select(_ => new IoAddressGroup([])) + .ToArray()); + + return new TrajectoryRequest(robot, program, PlanningMethod.Icsp); + } + + /// + /// 逐字段比较两套规划实现的数值结果。 + /// + /// 现有 C# ICSP 基准结果。 + /// Rust ICSP 原生结果。 + /// 所有浮点数组和 threshold 使用的绝对误差。 + private static void AssertTrajectoryMatches( + PlannedTrajectory managed, + PlannedTrajectory native, + double tolerance) + { + Assert.Equal(managed.Iterations, native.Iterations); + Assert.Equal(managed.WaypointTimes.Count, native.WaypointTimes.Count); + Assert.Equal(managed.SegmentDurations.Count, native.SegmentDurations.Count); + Assert.Equal(managed.SegmentScales.Count, native.SegmentScales.Count); + Assert.InRange(Math.Abs(managed.Threshold - native.Threshold), 0.0, tolerance); + + AssertSequenceMatches(managed.WaypointTimes, native.WaypointTimes, tolerance); + AssertSequenceMatches(managed.SegmentDurations, native.SegmentDurations, tolerance); + AssertSequenceMatches(managed.SegmentScales, native.SegmentScales, tolerance); + } + + /// + /// 按相同索引比较两个浮点序列的绝对误差。 + /// + /// 托管基准序列。 + /// Rust 原生序列。 + /// 允许的最大绝对误差。 + private static void AssertSequenceMatches( + IReadOnlyList expected, + IReadOnlyList actual, + double tolerance) + { + for (int index = 0; index < expected.Count; index++) + { + Assert.InRange(Math.Abs(expected[index] - actual[index]), 0.0, tolerance); + } + } +}