xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: xsxiaosa:dev
Branches Tags
xsxiaosa:master xsxiaosa:dev xsxiaosa:feat/rust-icsp-native xsxiaosa:legacy-gateway-compat
xsxiaosa:milestone/first-flyshot-run
..
compare: xsxiaosa:bcbf8017ad88c0b30c03081bea8c229b010d5c50
Branches Tags
xsxiaosa:dev xsxiaosa:feat/rust-icsp-native xsxiaosa:master xsxiaosa:legacy-gateway-compat
xsxiaosa:milestone/first-flyshot-run

5 Commits
dev ... bcbf8017ad

Author SHA1 Message Date
yunxiao.zhu
bcbf8017ad 📝 docs(planning): 统一 Rust 规划库命名 
将 Rust 原生规划库设计口径统一为 flyshot_planning。

同步更新 Windows 和 Linux 产物名以及复制命令示例。
 2026-06-27 15:10:06 +08:00
yunxiao.zhu
537c6b5320 💥 feat(planning)!: 拆分 C# ICSP 并默认 Rust 
* 新增可选 Flyshot.CSharp.Planning 项目

* 将 icsp 映射到 Rust 后端并拒绝旧 icsp_rust

* 补充后端注册、文档、调试页和回归测试

BREAKING CHANGE: method=icsp 现在默认调用 Rust 后端；C# ICSP 改为 method=icsp-csharp，旧 method=icsp_rust 不再作为正式入口。
 2026-06-27 15:03:05 +08:00
yunxiao.zhu
32cc740df5 ✨ feat(native-planning): 更新 Rust ICSP 原生库及构建脚本 
* 将 Rust ICSP 原生库名称更改为 `flyshot_planning`，更新相关引用。
* 修改构建脚本以支持新的库名称，并确保库文件复制到正确的目录。
* 添加对 Rust ICSP 原生库的依赖检查，确保发布时包含必要的库文件。
* 更新测试项目以加载新的原生库，确保兼容性和功能完整性。
 2026-06-26 17:47:04 +08:00
yunxiao.zhu
9c56ee825e ✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成 
* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口)
* 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装)
* 更新 ControllerClientCompat 适配新的规划接口
* 添加构建脚本 (Copy-NativePlanningArtifacts.ps1)
* 新增互操作测试及一致性对比测试
* 更新解决方案文件和设计文档
 2026-06-26 15:41:01 +08:00
yunxiao.zhu
bb21e0923d docs(planning): 添加 Rust ICSP 原生核心设计 2026-06-26 15:41:01 +08:00
65 changed files with 3719 additions and 1497 deletions
Expand all files Collapse all files

42
.claude/settings.local.json Normal file
View File

@@ -0,0 +1,42 @@
{
"permissions": {
"allow": [
"Bash(git commit -m ':*)",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln -v minimal 2>&1')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal 2>&1')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test FlyshotReplacement.sln --no-build -v minimal 2>&1')",
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal)",
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj --no-build -v minimal)",
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json'\\)\\); json.load\\(open\\('.vscode/tasks.json'\\)\\); print\\('JSON valid.'\\)\")",
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1')",
"Bash(taskkill /PID 120812 /F)",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build flyshot-replacement.sln --no-restore -v minimal 2>&1')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build /d/Dev/Codes/rvbust-code/FlyingShotPkg_3.15_VDA/flyshot-replacement/FlyshotReplacement.sln --no-restore -v minimal 2>&1')",
"Bash(capinfos \"20260430.pcap\")",
"Bash(tshark -r \"20260430.pcap\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e data.len)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.srcport -e udp.length -e data.len)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e udp.length)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e ip.dst -e udp.srcport -e udp.dstport -e udp.length -e frame.protocols)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and frame.number==1\" -V)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.dstport!=60015\" -T fields -e frame.number -e frame.time_relative -e ip.dst -e udp.dstport)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e ip.src -e udp.length)",
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp)",
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frj519.type -e frj519.seq -e frj519.status -e frj519.j1 -e frj519.j2 -e frj519.j3 -e frj519.j4 -e frj519.j5 -e frj519.j6 -e frj519.j7 -e frj519.j8 -e frj519.j9 -e frj519.timestamp -e frj519.x -e frj519.y -e frj519.z -e frj519.w -e frj519.p -e frj519.r)",
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -E header=y)",
"Bash(tshark -G fields)",
"Bash(tshark -r \"20260430.pcap\" -Y \"frj519\" -T fields -e frame.time_relative -e data)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
"Bash(tshark -r \"20260430.pcap\" -Y \"udp.port==60015 and udp.srcport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
"Bash(tshark -r \"20260430.pcap\" --disable-protocol frj519 -Y \"udp.dstport==60015\" -T fields -e frame.number -e frame.time_relative -e data)",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj --no-restore -v minimal 2>&1 | tail -20')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -25')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal 2>&1 | tail -15')",
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --no-restore -v minimal 2>&1 | tail -30')"
]
}
}

3
.gitignore vendored
View File

@@ -405,3 +405,6 @@ codex-dotnet-home/*
outputs/obfuscated/ outputs/obfuscated/
outputs/obfuscation-maps/ outputs/obfuscation-maps/
.tmp/obfuscation/ .tmp/obfuscation/
# Rust 原生规划库构建产物
**/target/

151
Config/RobotConfig.json
View File

@@ -10,8 +10,8 @@
"acc_limit": 1, "acc_limit": 1,
"jerk_limit": 1, "jerk_limit": 1,
"adapt_icsp_try_num": 5, "adapt_icsp_try_num": 5,
"planning_speed_scale": 0.74227,
"speed_ratio": 1, "speed_ratio": 1,
"planning_speed_scale": 0.74227,
"smooth_start_stop_timing": true "smooth_start_stop_timing": true
}, },
"flying_shots": { "flying_shots": {
@@ -300,155 +300,6 @@
], ],
[] []
] ]
},
"WhP_test": {
"traj_waypoints": [
[
1.056731,
0.011664811,
-0.017892333,
-0.01516874,
0.021492079,
0.009567846
],
[
0.2153671,
0.015257306,
-0.024938405,
-0.059105303,
0.06653474,
-0.005172012
],
[
0.47362572,
0.015260376,
-0.024971163,
-0.059100218,
0.06653733,
-0.00518099
],
[
-0.0861789,
0.024431378,
0.15767263,
-0.059098743,
0.06653532,
-0.0051812795
],
[
-0.029219253,
0.024431676,
0.24181713,
-0.81706756,
0.06653719,
-0.0051850453
],
[
1.056731,
0.011664811,
-0.017892333,
-0.01516874,
0.021492079,
0.009567846
]
],
"shot_flags": [
false,
true,
true,
true,
true,
false
],
"offset_values": [
0,
0,
0,
0,
0,
0
],
"addr": [
[],
[
2,
3,
4
],
[
2,
4,
3
],
[
1,
2,
3
],
[
1,
2,
3
],
[]
]
},
"demo-flyshot": {
"traj_waypoints": [
[
0,
0,
0,
0,
0,
0
],
[
0.001,
0,
0,
0,
0,
0
],
[
0.002,
0,
0,
0,
0,
0
],
[
0.003,
0,
0,
0,
0,
0
]
],
"shot_flags": [
false,
true,
false,
false
],
"offset_values": [
0,
1,
0,
0
],
"addr": [
[],
[
7,
8
],
[],
[]
]
} }
} }
} }

31
FlyshotReplacement.sln
View File

@@ -1,4 +1,3 @@

Microsoft Visual Studio Solution File, Format Version 12.00 Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17 # Visual Studio Version 17
VisualStudioVersion = 17.0.31903.59 VisualStudioVersion = 17.0.31903.59
@@ -27,6 +26,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.ControllerClientCom
EndProject EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}" Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}"
EndProject EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Native.Planning", "src\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj", "{928CF396-C0E1-488E-BCC1-CF0279CF2DD0}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.CSharp.Planning", "src\Flyshot.CSharp.Planning\Flyshot.CSharp.Planning.csproj", "{4717558C-0A09-4D4D-BB1A-C0FD9241070E}"
EndProject
Global Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU Debug|Any CPU = Debug|Any CPU
@@ -157,6 +160,30 @@ Global
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.Build.0 = Release|Any CPU {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.ActiveCfg = Release|Any CPU
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.Build.0 = 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
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|Any CPU.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x64.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x64.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x86.ActiveCfg = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Debug|x86.Build.0 = Debug|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|Any CPU.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|Any CPU.Build.0 = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x64.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x64.Build.0 = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x86.ActiveCfg = Release|Any CPU
{4717558C-0A09-4D4D-BB1A-C0FD9241070E}.Release|x86.Build.0 = Release|Any CPU
EndGlobalSection EndGlobalSection
GlobalSection(SolutionProperties) = preSolution GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE HideSolutionNode = FALSE
@@ -172,5 +199,7 @@ Global
{E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3} {E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3} = {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} {B705FA6C-19CA-44A8-882C-6CE26A5379C9} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{928CF396-C0E1-488E-BCC1-CF0279CF2DD0} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{4717558C-0A09-4D4D-BB1A-C0FD9241070E} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
EndGlobalSection EndGlobalSection
EndGlobal EndGlobal

35
README.md
View File

@@ -16,7 +16,8 @@
- 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口；旧 `ControllerClient` 逆向资料只作为接口语义参考，不作为运行时目标。 - 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口；旧 `ControllerClient` 逆向资料只作为接口语义参考，不作为运行时目标。
- 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。 - 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。 - `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。 - `Flyshot.Core.Planning` 保留规划公共模型、样条、采样、导出和运动学能力；C# ICSP / self-adapt-icsp 已拆分到可选项目 `Flyshot.CSharp.Planning`。
- `Flyshot.Native.Planning` 已提供默认 Rust ICSP 规划入口，通过 C ABI 调用 `flyshot_planning`；普通轨迹和飞拍轨迹的默认 `method="icsp"` 使用 Rust 后端，`method="icsp-csharp"` 和 `method="self-adapt-icsp"` 只有显式注册 C# 后端时可用。
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。 - `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-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 内部参数限制规划加速度。 - 2026-04-30 本机 `50001/TCP+JSON` 抓包确认：`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数，不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路，新系统按 replacement-only 内部参数限制规划加速度。
@@ -64,8 +65,9 @@
- [x] 打通最小宿主与 `/healthz` - [x] 打通最小宿主与 `/healthz`
- [x] 建立领域模型与模块边界 - [x] 建立领域模型与模块边界
- [x] 落地配置兼容与机器人模型解析 - [x] 落地配置兼容与机器人模型解析
- [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴 - [x] 落地 Rust ICSP、可选 C# ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴
- [x] 完成 ICSP 轨迹导出结果与旧系统对齐 - [x] 完成 ICSP 轨迹导出结果与旧系统对齐
- [x] 增加 Rust `cdylib` ICSP 核心、C# P/Invoke 适配和 C#/Rust 自动对拍
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路 - [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路
- [x] 落地 Web 状态页 - [x] 落地 Web 状态页
- [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页（`/debug`），与 `Swagger:Enabled` 同步可见 - [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页（`/debug`），与 `Swagger:Enabled` 同步可见
@@ -79,12 +81,38 @@
- [x] 完成真机优先与离线飞拍规划改造：`RealReady / OfflineReady / ModelLoadFailed` 状态、固定轨迹资产、完整离线规划响应和 Debug 页面 - [x] 完成真机优先与离线飞拍规划改造：`RealReady / OfflineReady / ModelLoadFailed` 状态、固定轨迹资产、完整离线规划响应和 Debug 页面
- [x] 删除生产仿真切换 HTTP 入口及旧 `/init_mpc_robt`，统一使用 `/init_mpc_robot` - [x] 删除生产仿真切换 HTTP 入口及旧 `/init_mpc_robt`，统一使用 `/init_mpc_robot`
### 规划后端与 Rust ICSP 原生库构建
普通轨迹或飞拍轨迹的默认参数 `method="icsp"` 会调用 Rust ICSP 后端。旧 `method="icsp_rust"` 已不再作为正式入口，调用时会提示改用 `method="icsp"`；Rust 原生库缺失时会明确报错，不会静默回退 C#。
C# `ICspPlanner` 与 `SelfAdaptIcspPlanner` 已拆分到 `Flyshot.CSharp.Planning.dll`。默认服务项目不引用该 DLL；只有在部署方显式引用并调用 `AddCSharpPlanningBackends()` 后，`method="icsp-csharp"` 与 `method="self-adapt-icsp"` 才可用。
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
```
脚本会执行 `cargo build --release`，构建
`native/flyshot-native-planning/target/release/flyshot_planning.dll`，
并默认复制到 `src/Libs/flyshot_planning.dll` 与
`tests/Flyshot.Core.Tests/bin/Release/net8.0/`。发布服务时可通过
`-DestinationDirectories` 把原生库复制到实际发布目录。
Linux x64 在已安装 `stable-x86_64-unknown-linux-gnu` 工具链的 Linux 环境执行同一脚本，
产物为 `libflyshot_planning.so`，默认写入 `src/Libs/libflyshot_planning.so`。
Windows 第一阶段不要求交叉编译 Linux 原生库。
剩余 Todo： 剩余 Todo：
1. 配置与测试基线 1. 配置与测试基线
- [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移：`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`，应改用稳定样本或更新断言。 - [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移：`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`，应改用稳定样本或更新断言。
- [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。 - [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。
- [x] 恢复 `use_do` 的输出选择语义：`true` 使用 DO（2），`false` 使用 RO（9），两种模式均保留飞拍脉冲与清零；见 [输出选择说明](docs/use-do-output-selection.md)。
- [x] 将上传飞拍轨迹统一保存到运行目录 `Config/RobotConfig.json` 的 `flying_shots` 节点。 - [x] 将上传飞拍轨迹统一保存到运行目录 `Config/RobotConfig.json` 的 `flying_shots` 节点。
- [x] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流：见 `docs/fanuc-field-runtime-workflow.md`。 - [x] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流：见 `docs/fanuc-field-runtime-workflow.md`。
@@ -118,7 +146,6 @@
- [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。 - [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。
- [x] 将 `MoveJoint` 从单点最终目标改为临时 PTP 稠密轨迹：按 `status=15` 运动窗口统计，speed=1 抓包 40 点，speed=0.7 抓包 55 点，speed=0.5 抓包 77 点，路径为关节空间直线 + smoothstep 进度。 - [x] 将 `MoveJoint` 从单点最终目标改为临时 PTP 稠密轨迹：按 `status=15` 运动窗口统计，speed=1 抓包 40 点，speed=0.7 抓包 55 点，speed=0.5 抓包 77 点，路径为关节空间直线 + smoothstep 进度。
- [x] `ExecuteFlyShotTraj(move_to_start=true)` 复用临时 PTP 稠密轨迹移动到规划起点，并等待运行时完成后再启动飞拍轨迹，避免第一帧 J519 目标突变导致控制柜报警。 - [x] `ExecuteFlyShotTraj(move_to_start=true)` 复用临时 PTP 稠密轨迹移动到规划起点，并等待运行时完成后再启动飞拍轨迹，避免第一帧 J519 目标突变导致控制柜报警。
- [x] `MoveJoint` 和飞拍预移动统一按 `planning_speed_scale * speed_ratio` 生成点对点轨迹，速度、加速度、跃度分别按有效倍率的一、二、三次方缩放，发送周期保持不变。
- [x] `ExecuteFlyShotTraj(wait=true)` 等待正式飞拍轨迹执行完成后再返回；HTTP `/execute_flyshot/` 已接入旧抓包中的 `wait` 字段，默认值为 `true`。 - [x] `ExecuteFlyShotTraj(wait=true)` 等待正式飞拍轨迹执行完成后再返回；HTTP `/execute_flyshot/` 已接入旧抓包中的 `wait` 字段，默认值为 `true`。
6. 真机联调与运行安全 6. 真机联调与运行安全

76
build/Copy-NativePlanningArtifacts.ps1 Normal file
View File

@@ -0,0 +1,76 @@
<#
.SYNOPSIS
构建 Rust ICSP 原生规划库并复制到 src/Libs 与可选目标目录。
.DESCRIPTION
在 Windows x64 上通过 cargo release 构建 flyshot_planning，默认写入
src/Libs/flyshot_planning.dll；Linux x64 写入 src/Libs/libflyshot_planning.so。
可通过 -DestinationDirectories 额外复制到测试或发布输出目录。
.PARAMETER Configuration
传递给 dotnet 相关路径的构建配置名，默认 Release。
.PARAMETER DestinationDirectories
除 src/Libs 外需要覆盖复制的额外目标目录列表。
#>
[CmdletBinding()]
param(
[ValidateSet('Debug', 'Release')]
[string]$Configuration = 'Release',
[string[]]$DestinationDirectories = @()
)
$ErrorActionPreference = 'Stop'
Set-StrictMode -Version Latest
$repositoryRoot = (Resolve-Path (Join-Path $PSScriptRoot '..')).Path
$cargoManifest = Join-Path $repositoryRoot 'native\flyshot-native-planning\Cargo.toml'
$libsDirectory = Join-Path $repositoryRoot 'src\Libs'
$cargoProfile = if ($Configuration -eq 'Debug') { 'debug' } else { 'release' }
$targetDirectory = Join-Path $repositoryRoot "native\flyshot-native-planning\target\$cargoProfile"
if ($IsLinux -or (-not $IsWindows -and -not $IsMacOS))
{
$artifactFileName = 'libflyshot_planning.so'
}
else
{
$artifactFileName = 'flyshot_planning.dll'
}
$builtArtifactPath = Join-Path $targetDirectory $artifactFileName
$libsArtifactPath = Join-Path $libsDirectory $artifactFileName
Write-Host "正在构建 Rust 原生规划库 ($Configuration) ..."
& cargo build --manifest-path $cargoManifest --$Configuration
if ($LASTEXITCODE -ne 0)
{
throw "cargo build 失败，退出码：$LASTEXITCODE"
}
if (-not (Test-Path -LiteralPath $builtArtifactPath -PathType Leaf))
{
throw "未找到构建产物：$builtArtifactPath"
}
New-Item -ItemType Directory -Path $libsDirectory -Force | Out-Null
Copy-Item -LiteralPath $builtArtifactPath -Destination $libsArtifactPath -Force
Write-Host "已复制到：$libsArtifactPath"
$defaultTestOutput = Join-Path $repositoryRoot "tests\Flyshot.Core.Tests\bin\$Configuration\net8.0"
$allDestinations = @($DestinationDirectories) + @($defaultTestOutput)
foreach ($destination in $allDestinations | Where-Object { -not [string]::IsNullOrWhiteSpace($_) } | Select-Object -Unique)
{
$absoluteDestination = [System.IO.Path]::GetFullPath($destination)
if (-not (Test-Path -LiteralPath $absoluteDestination -PathType Container))
{
Write-Host "跳过不存在的目标目录：$absoluteDestination"
continue
}
Copy-Item -LiteralPath $builtArtifactPath -Destination (Join-Path $absoluteDestination $artifactFileName) -Force
Write-Host "已复制到：$absoluteDestination\$artifactFileName"
}

109
docs/collaborative-robot-flyshot-start-api.md
View File

@@ -1,109 +0,0 @@
# 协作机器人飞拍接口
本文仅保留协作机器人后端需要调用的两个接口：设置运行速度、启动已存在的飞拍任务。不包含初始化、连接、使能、任务上传或任务管理。
服务地址示例：`http://<飞拍服务地址>:<端口>`。
## 1. 设置运行速度
```http
POST /set_speedRatio/
Content-Type: application/json
```
请求体：
```json
{
"speed": 0.7
}
```
| 字段 | 类型 | 范围 | 说明 |
| --- | --- | --- | --- |
| `speed` | number | `0.01` ～ `1.0` | 运行速度倍率；`1.0` 为全速，`0.7` 为 70% 速度。 |
成功响应：`200 OK`
```json
{
"message": "set_speedRatio executed",
"returnCode": 0
}
```
参数不在范围内时返回 `400 Bad Request`：
```json
{
"detail": "Speed ratio must be between 0.01 and 1.0."
}
```
应在启动飞拍前设置速度。当前接口不约定任务执行过程中的动态改速。
## 2. 启动飞拍任务
```http
POST /execute_flyshot/
Content-Type: application/json
```
请求体：
```json
{
"name": "协作机器人飞拍任务名",
"move_to_start": true,
"method": "icsp",
"save_traj": false,
"use_cache": true,
"wait": false
}
```
| 字段 | 类型 | 说明 |
| --- | --- | --- |
| `name` | string | 已存在的飞拍任务名称。 |
| `move_to_start` | boolean | 是否先移动到飞拍轨迹起点；建议为 `true`。 |
| `method` | string | 轨迹规划方法；使用 `"icsp"`。 |
| `save_traj` | boolean | 是否保存规划导出信息；启动任务时使用 `false`。 |
| `use_cache` | boolean | 是否复用规划缓存；建议为 `true`。 |
| `wait` | boolean | `false` 表示启动后立即返回；本接口使用 `false`。 |
成功响应：`200 OK`
```json
{
"status": "FlyShot executed",
"success": true
}
```
`wait=false` 时，`200` 仅表示任务已接受并开始调度，不表示飞拍已经完成。
启动失败时返回 `500 Internal Server Error`：
```json
{
"detail": "具体失败原因"
}
```
## 3. 调用顺序
```text
POST /set_speedRatio/ → 收到 returnCode = 0 → POST /execute_flyshot/
```
curl 示例：
```bash
curl -X POST 'http://<飞拍服务地址>:<端口>/set_speedRatio/' \
-H 'Content-Type: application/json' \
-d '{"speed":0.7}'
curl -X POST 'http://<飞拍服务地址>:<端口>/execute_flyshot/' \
-H 'Content-Type: application/json' \
-d '{"name":"协作机器人飞拍任务名","move_to_start":true,"method":"icsp","save_traj":false,"use_cache":true,"wait":false}'
```

6
docs/controller-client-api-compatibility-requirements.md
View File

@@ -130,11 +130,11 @@
以下内容是当前新实现的状态，不反推为旧系统事实： 以下内容是当前新实现的状态，不反推为旧系统事实：
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。 - `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。 - `ExecuteTrajectory` 会先通过默认 Rust `method="icsp"` 后端规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。 - `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过已注册的规划后端生成轨迹，并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
- HTTP 控制器已经按公开文档和抓包补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache, wait)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。 - HTTP 控制器已经按公开文档和抓包补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache, wait)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
- 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束，replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度；该字段不属于旧 `ControllerClient` 公开 API，也不会通过 `50001` JSON 下发。 - 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束，replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度；该字段不属于旧 `ControllerClient` 公开 API，也不会通过 `50001` JSON 下发。
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。 - `method="icsp"` 已接入默认 Rust 后端；`method="icsp-csharp"` 与 `method="self-adapt-icsp"` 只有显式注册 `Flyshot.CSharp.Planning` 后端时可用；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。 - `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。 - `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 - 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。

573
docs/flyshot-system-guide.md
View File

@@ -1,573 +0,0 @@
# 飞拍系统介绍说明与使用教程
## 1. 文档目的
本文档用于说明当前飞拍系统的整体组成、数据流、接口边界和现场使用步骤。文档原稿采用 Markdown 编写，适合后续导入或整理为飞书文档；流程图使用 Mermaid 源码保留，导入飞书时可按需要渲染为图片后插入。
系统当前分为三个主要单元：
| 单元 | 工程位置 | 主要职责 |
| --- | --- | --- |
| 上位机单元 | `D:\Dev\Codes\MFD_Solution\FlyShot` | 控制相机、接收 PLC/MES 信息、发起飞拍任务、保存图片、调用 AI 检测、汇总结果并回传 MES/PLC |
| 飞拍控制单元 | `D:\Dev\Codes\rvbust-code\FlyingShotPkg_3.15_VDA\flyshot-replacement` | 提供 HTTP 控制接口，负责机器人初始化、轨迹上传、ICSP/self-adapt-ICSP 规划、触发时序生成和 FANUC 运动执行 |
| YOLO AI 识别模块 | 由上位机配置访问 | 接收图片检测请求，完成 AI 推理，返回图片级 OK/NG、原因、参数明细和结果图路径 |
## 2. 系统总览
```mermaid
flowchart LR
PLC["PLC / OPC UA<br/>任务触发、状态反馈"]
MES["MES<br/>换型下发、PartProcessed 上报"]
PC["上位机单元 FlyShot<br/>WPF 主控程序"]
Camera["工业相机<br/>采图与本地存图"]
Control["飞拍控制单元<br/>ASP.NET Core HTTP API"]
Robot["FANUC 机器人<br/>TCP 10010/10012 + UDP 60015"]
AI["YOLO AI 识别模块<br/>HTTP + 文件传输目录"]
Storage["本地/远端图片目录<br/>原图、结果图、任务 JSON"]
Notify["飞书 / 审核平台 / ClickHouse<br/>异常通知与追溯"]
PLC <--> PC
MES <--> PC
PC --> Camera
Camera --> PC
PC <--> Control
Control <--> Robot
PC <--> AI
PC --> Storage
AI --> Storage
PC --> Notify
```
### 2.1 上位机单元
上位机是现场操作和生产流程的核心入口，当前是 WPF MVVM 程序。它负责：
- 初始化相机并注册图像回调。
- 读取 `appConfig.json`、`Config\TypeData`、`Config\FlyTask` 中的现场配置。
- 通过 `MpcService` 调用飞拍控制单元 HTTP 接口。
- 按飞拍任务配置设置相机曝光、增益和拍摄索引。
- 将图片保存到 `CameraSavePath` 下的任务目录。
- 通过 `ImageProcessingService` 上传图片到 AI 服务器目录并调用 AI 接口。
- 将 AI 结果合并回任务对象，保存任务 JSON。
- 根据结果回写 PLC、发送 MES `PartProcessed`、写入 ClickHouse、必要时发送飞书 NG 报警。
### 2.2 飞拍控制单元
飞拍控制单元是新的 replacement 服务端。它不恢复旧 `50001/TCP+JSON` 网关，而是提供 ASP.NET Core HTTP API。它负责：
- 机器人模型加载与初始化。
- 轨迹上传、查询、删除。
- 普通轨迹 `ExecuteTrajectory`。
- 飞拍轨迹 `ExecuteFlyShotTraj`。
- 通过 `icsp` 或 `self-adapt-icsp` 生成关节轨迹。
- 根据 `shot_flags / offset_values / addrs` 生成拍照触发时间轴。
- 将规划结果转换为 8ms 发送队列，通过 J519 通道控制机器人运动和 IO 脉冲。
- 提供 `/status.html` 状态页和 `/debug.html` 调试页。
### 2.3 YOLO AI 识别模块
YOLO AI 模块由上位机调用。上位机当前不是直接把所有图片以 multipart 方式传给 AI，而是：
1. 将图片上传到配置的 AI 输入目录。
2. 构造包含模型版本、配置版本、产品 ID 和图片明细的 JSON 请求。
3. 调用 `Ai_Model.ApiUrl`。
4. 根据 AI 响应中的 `SubImgInfo` 下载结果图。
5. 将图片级结果、原因、参数明细写回任务结果。
6. 清理远端上传图和结果图。
## 3. 生产主流程
```mermaid
sequenceDiagram
participant PLC as PLC
participant PC as 上位机 FlyShot
participant CTRL as 飞拍控制单元
participant ROBOT as FANUC 机器人
participant CAM as 工业相机
participant AI as YOLO AI
participant MES as MES
PLC->>PC: StartFlyShotTask 上升沿 + 产品/任务信息
PC->>PC: 初始化 WorkStationTask / FlyShotTask
PC->>CAM: 注册回调、设置曝光/增益
PC->>CTRL: POST /execute_flyshot/
CTRL->>ROBOT: move_to_start + J519 飞拍轨迹
ROBOT-->>CAM: 轨迹 IO 脉冲触发拍照
CAM-->>PC: 图像回调
PC->>PC: 保存原图并加入待处理队列
PC->>AI: 上传图片 + 发送 AI 请求
AI-->>PC: 返回 OK/NG、原因、结果图路径
PC->>PC: 下载结果图并汇总任务结果
PC-->>PLC: TaskCompleted / TaskResult / WorkStationResult
PC->>MES: PartProcessed 结果报文
```
## 4. 关键配置
### 4.1 上位机配置
上位机配置文件：
- 基础配置：`D:\Dev\Codes\MFD_Solution\FlyShot\appConfig.json`
- 调试覆盖：`D:\Dev\Codes\MFD_Solution\FlyShot\appConfig.Debug.json`
常用配置项：
| 配置路径 | 示例值 | 说明 |
| --- | --- | --- |
| `MesConfig.EnableMes` | `true` | 是否启用 MES 通信 |
| `MesConfig.ServerIp` / `ServerPort` | `174.34.82.193` / `55065` | MES 服务地址 |
| `MesConfig.LineNo` / `ProcessNo` / `StatNo` | `6958` / `993002` / `9930` | MES 位置上下文 |
| `MesConfig.TelegramFolder` | `D:\MesTelegram` | MES 报文保存目录 |
| `CameraIp` / `LocalIp` | `192.168.11.11` / `192.168.11.188` | 工业相机与本机网卡地址 |
| `CameraSavePath` | `D:\Images\flyShot` | 飞拍原图、结果图和任务 JSON 保存根目录 |
| `MPC.MpcInterfaceIp` | `192.168.11.12` | 飞拍控制单元 HTTP 地址 |
| `MPC.MpcInterfacePort` | `9001` | 上位机调用的飞拍控制单元端口 |
| `MPC.RobotName` | `LR_Mate_200iD_7L` | 机器人型号/名称 |
| `MPC.RobotIp` | `192.168.10.11` | FANUC 控制柜 IP |
| `Ai_Model.ApiUrl` | `http://127.0.0.1:8124/corebg` | AI HTTP 接口地址 |
| `Ai_Model.AI_Model_Version` | `PEU` | AI 模型版本 |
| `Ai_Model.AI_configVersion` | `peu_20241011` | AI 配置版本 |
| `Ai_Model.ai_upload_path` | `/mnt/d/ai_images/ai-feed` | AI 输入图片目录 |
| `Ai_Model.ai_result_path` | `/mnt/d/ai_images` | AI 结果图片目录 |
| `FileTransfer.DefaultService` | `Primary` | 当前使用的文件传输服务 |
| `PLC.EnablePlc` | `true` | 是否启用 PLC 通信 |
| `PLC.OpcUaServer` | `opc.tcp://174.34.178.68:4840` | OPC UA 服务地址 |
> 注意：如果飞拍控制单元实际监听端口是 `5000`，上位机的 `MPC.MpcInterfacePort` 需要同步改为 `5000`，或通过反向代理/部署配置把控制单元暴露为 `9001`。
### 4.2 飞拍控制单元配置
飞拍控制单元配置文件：
- `D:\Dev\Codes\rvbust-code\FlyingShotPkg_3.15_VDA\flyshot-replacement\src\Flyshot.Server.Host\appsettings.json`
常用配置项：
| 配置路径 | 示例值 | 说明 |
| --- | --- | --- |
| `ControllerClientCompat.AutoInit.RobotName` | `FANUC_LR_Mate_200iD_7L` | 服务启动自动初始化的机器人名称 |
| `ControllerClientCompat.AutoInit.RobotIp` | `192.168.10.11` | 服务启动自动连接的机器人 IP |
| `Kestrel.Endpoints.Http.Url` | `http://0.0.0.0:5000` | HTTP 服务监听地址 |
| `Swagger.Enabled` | `true` | 是否启用 Swagger 和调试接口 |
| `Cors.AllowedOrigins` | `*` | 允许跨域调用的来源 |
## 5. 飞拍控制单元 HTTP 接口
### 5.1 初始化机器人
推荐使用聚合初始化接口：
```http
POST /init_mpc_robot
Content-Type: application/json
{
"robot_name": "FANUC_LR_Mate_200iD_7L",
"robot_ip": "192.168.10.11"
}
```
内部执行顺序：
1. `SetUpRobot`：加载机器人模型和关节限制。
2. `Connect`：建立 FANUC 状态、命令、J519 三条通道。
3. `EnableRobot(8)`：启动机器人程序和运动循环。
也可以拆分调用：
```text
POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD_7L
POST /connect_robot/?ip=192.168.10.11
GET /enable_robot/?buffer_size=8
```
### 5.2 上传飞拍轨迹
```http
POST /upload_flyshot/
Content-Type: application/json
{
"name": "UTTC_MS11",
"waypoints": [[0, 0, 0, 0, 0, 0]],
"shot_flags": [true],
"offset_values": [0],
"addrs": [[1, 3]]
}
```
字段说明：
| 字段 | 类型 | 说明 |
| --- | --- | --- |
| `name` | string | 飞拍轨迹名称，上位机执行时使用 |
| `waypoints` | number[][] | 机器人关节路径点 |
| `shot_flags` | bool[] | 每个路径点是否拍照 |
| `offset_values` | number[] | 拍照触发偏移周期 |
| `addrs` | number[][] | 拍照触发 IO 地址组 |
### 5.3 执行飞拍轨迹
上位机 `MpcService.ExecuteFlyShot` 当前默认发送：
```http
POST /execute_flyshot/
Content-Type: application/json
{
"name": "UTTC_MS11",
"move_to_start": true,
"method": "icsp",
"save_traj": false,
"use_cache": false,
"wait": false
}
```
字段说明：
| 字段 | 推荐值 | 说明 |
| --- | --- | --- |
| `name` | 轨迹名称 | 必须已上传或存在于控制单元缓存 |
| `move_to_start` | `true` | 执行前先移动到规划起点，避免第一帧目标突变 |
| `method` | `icsp` 或 `self-adapt-icsp` | 轨迹规划方法 |
| `save_traj` | `false` | 是否保存本次规划轨迹 |
| `use_cache` | `false` 或 `true` | 是否复用缓存轨迹 |
| `wait` | `false` 或 `true` | 是否等待整条轨迹完成后再返回 |
### 5.4 常用调试接口
| 接口 | 方法 | 用途 |
| --- | --- | --- |
| `/healthz` | GET | 服务健康检查 |
| `/status.html` | GET | 浏览器状态页 |
| `/debug.html` | GET | 浏览器接口调试页 |
| `/api/status/snapshot` | GET | 当前兼容层和运行时状态快照 |
| `/robot_info/` | GET | 查询机器人信息 |
| `/set_speedRatio/` | POST | 设置速度倍率 |
| `/list_flyShotTraj/` | GET | 查询可执行飞拍轨迹名称 |
| `/delete_flyshot/` | POST | 删除飞拍轨迹 |
| `/stop_move/` | GET | 停止当前运动 |
| `/disable_robot/` | GET | 禁用机器人 |
| `/disconnect_robot/` | POST | 断开机器人连接 |
## 6. 上位机使用教程
### 6.1 启动前检查
1. 确认工业相机、机器人控制柜、AI 服务、PLC、MES 网络可达。
2. 检查上位机 `appConfig.json`：
- `CameraIp` 和 `LocalIp` 是否匹配现场网段。
- `CameraSavePath` 是否存在或可创建。
- `MPC.MpcInterfaceIp` / `MpcInterfacePort` 是否指向飞拍控制单元。
- `Ai_Model.ApiUrl` 是否指向 YOLO AI 接口。
- `FileTransfer` 是否能访问 AI 上传和结果目录。
3. 检查飞拍控制单元是否启动：
- 浏览器访问 `http://<控制单元IP>:<端口>/healthz`。
- 浏览器访问 `http://<控制单元IP>:<端口>/status.html`。
4. 检查机器人程序、控制柜状态和安全围栏。
### 6.2 上位机启动流程
```mermaid
flowchart TD
A["启动 FlyShot 上位机"] --> B["读取 appConfig.json"]
B --> C["初始化 MES<br/>可按 EnableMes 关闭"]
B --> D["初始化相机"]
B --> E["查询飞拍控制单元轨迹列表"]
E --> F["设置机器人速度倍率<br/>默认 0.5"]
D --> G["确认系统状态栏无关键 NG"]
F --> G
G --> H["等待手动或 PLC 触发任务"]
```
启动后，上位机会执行：
- 初始化 MES 连接。
- 初始化相机。
- 查询飞拍控制单元中的飞拍任务列表。
- 设置机器人速度倍率。
- 创建或检查图片保存目录。
### 6.3 手动执行飞拍任务
适用于调试、首件验证和现场排查。
1. 打开上位机。
2. 确认相机初始化成功。
3. 确认飞拍任务列表已加载。
4. 输入或扫码产品标识。
5. 选择飞拍任务名称。
6. 点击手动开始。
7. 上位机执行：
- 初始化工位任务。
- 注册相机回调。
- 调用飞拍控制单元 `/execute_flyshot/`。
- 接收相机回调并保存图片。
- 批量调用 AI 检测。
- 汇总 OK/NG。
- 保存任务 JSON。
- 如有 NG，打开人工复判界面并发送通知。
### 6.4 PLC 自动执行飞拍任务
适用于正式生产。
PLC 通过 OPC UA 写入任务触发信号，上位机收到 `StartFlyShotTask` 上升沿后执行任务。
PLC 输入给 PC 的核心信号：
| 信号 | 说明 |
| --- | --- |
| `StartFlyShotTask` | 飞拍任务启动，上升沿触发 |
| `WorkStationId` | 工位号 |
| `FlyShotTaskName` | 飞拍任务名称 |
| `CurrentTaskIndex` | 当前任务序号 |
| `TotalTaskCount` | 当前工位任务总数 |
| `ProductTypeNo` | 产品型号 |
| `ProductIdentifier` | 产品唯一标识 |
| `ProductBatch` | 产品批次 |
| `ResetSystem` | 系统重置信号 |
PC 回写给 PLC 的核心信号：
| 信号 | 说明 |
| --- | --- |
| `TaskCompleted` | 当前任务完成 |
| `TaskResult` | 当前任务结果，true=OK，false=NG |
| `CompletedTaskIndex` | 已完成任务序号 |
| `TaskProcessingTime` | 当前任务耗时 |
| `WorkStationCompleted` | 工位所有任务是否完成 |
| `WorkStationResult` | 工位总结果 |
| `SystemReady` | 系统是否就绪 |
| `SystemBusy` | 系统是否忙碌 |
| `ErrorCode` | 错误码 |
| `ErrorMessage` | 错误信息 |
| `PcHeartbeat` | PC 心跳 |
### 6.5 图片和结果保存规则
上位机会基于 `CameraSavePath` 生成任务目录：
```text
{CameraSavePath}\{yyyyMMdd}\{TypeNo-TypeVar}\{ProductIdentifier}\{FlyShotName}\
```
任务目录内通常包含：
| 内容 | 说明 |
| --- | --- |
| 原始图片 | 相机回调保存的图片 |
| `result` 子目录 | AI 下载后的结果图 |
| `flyShotTask@...json` | 单个飞拍任务详情 |
| `WorkStationTask@...json` | 工位任务汇总 |
AI 结果图命名会带上检测结果，例如：
```text
原图名@OK.jpeg
原图名@NG.jpeg
```
## 7. YOLO AI 识别流程
```mermaid
flowchart TD
A["相机回调保存原图"] --> B["上位机收集待处理图片"]
B --> C["达到 BatchSize 或采集结束"]
C --> D["上传原图到 Ai_Model.ai_upload_path"]
D --> E["构造 AiProcessRequestModel"]
E --> F["POST Ai_Model.ApiUrl"]
F --> G["解析 ProductResult / SubImgInfo / FailureModes"]
G --> H["从 Ai_Model.ai_result_path 下载结果图"]
H --> I["写回 ImageProcessOutcome"]
I --> J["更新 UI 与任务结果"]
J --> K["清理 AI 远端临时文件"]
```
### 7.1 AI 请求结构
AI 请求的典型结构：
```json
{
"ai_modelVersion": "PEU",
"ai_configVersion": "peu_20241011",
"key": "empty",
"product_id": "20250228121929",
"sub_img_info": [
{
"index": 1,
"view": "pipe",
"object": "spring",
"image_path": "000001_161008_1.jpeg",
"ai_image_path": null,
"ai_result": null,
"ai_reason": null,
"ai_param_list:": {}
}
]
}
```
字段说明：
| 字段 | 说明 |
| --- | --- |
| `ai_modelVersion` | AI 模型版本，对应上位机 `Ai_Model.AI_Model_Version` |
| `ai_configVersion` | AI 配置版本，对应上位机 `Ai_Model.AI_configVersion` |
| `product_id` | 本次 AI 任务 ID，通常由时间戳生成 |
| `sub_img_info[].index` | 图片序号 |
| `sub_img_info[].view` | 视角，例如 `pipe`、`surface`、`connector` |
| `sub_img_info[].object` | 检测对象，例如 `spring`、`top`、`side` |
| `sub_img_info[].image_path` | 上传到 AI 输入目录后的图片文件名 |
### 7.2 AI 响应处理
上位机按 `index` 将 AI 响应和原图对应起来，主要读取：
| 字段 | 说明 |
| --- | --- |
| `ProductResult` | 产品级检测结果 |
| `ErrorInfo` | AI 处理错误信息 |
| `AiFailureModel` | AI 不良模式集合 |
| `SubImgInfo[].AiResult` | 单张图片结果，`OK` 视为 OK，其他非空值视为 NG |
| `SubImgInfo[].AiReason` | 单张图片 NG 原因 |
| `SubImgInfo[].AiParamList` | AI 参数明细 |
| `SubImgInfo[].AiImagePath` | AI 结果图远端路径 |
## 8. MES 交互说明
### 8.1 换型下发
上位机启动或手动换型时，会根据 `MesConfig.TypeNo` 和 `MesConfig.TypeVar` 发送 `DataDownloadRequired`。
主要用途：
- 获取或刷新当前型号配置。
- 更新本地 `TypeNo` / `TypeVar`。
- 加载对应 `Config\TypeData` 文件。
### 8.2 结果上报
飞拍任务完成后，上位机会发送 `PartProcessed`。
上报内容包括：
- 产品唯一标识。
- 产品型号和变体。
- 工位结果，`1=OK`，`2=NG`。
- 图片相对路径。
- 每张图片的 AI 结果和人工复判结果。
- 汇总后的 NIO bits。
## 9. 常见使用场景
### 9.1 首次联调
1. 启动飞拍控制单元。
2. 访问 `/healthz` 确认服务正常。
3. 访问 `/status.html` 查看模型和机器人状态。
4. 调用 `/init_mpc_robot` 初始化机器人。
5. 调用 `/list_flyShotTraj/` 确认可用轨迹。
6. 启动上位机。
7. 检查上位机日志中相机、MPC、MES、PLC 状态。
8. 使用手动模式执行一次飞拍任务。
9. 检查图片目录、AI 结果图、任务 JSON 和 MES/PLC 反馈。
### 9.2 正式生产
1. 启动飞拍控制单元并确认状态正常。
2. 启动上位机并确认系统就绪。
3. PLC 写入产品和任务信息。
4. PLC 触发 `StartFlyShotTask` 上升沿。
5. 上位机执行飞拍、AI 检测和结果汇总。
6. 上位机回写 PLC 当前任务和工位结果。
7. 上位机发送 MES `PartProcessed`。
8. PLC 根据 `WorkStationCompleted` 和 `WorkStationResult` 决定后续流程。
### 9.3 离线排查 AI
1. 准备一组本地图片。
2. 在上位机调试页面或相关调试入口选择图片。
3. 确认 `Ai_Model.ApiUrl`、`ai_upload_path`、`ai_result_path` 正确。
4. 执行 AI 请求。
5. 检查 AI 响应、结果图下载、上位机日志和远端文件清理情况。
## 10. 故障排查
```mermaid
flowchart TD
A["发现任务失败或结果异常"] --> B{"失败阶段"}
B -->|相机无图| C["检查 CameraIp / LocalIp / 相机连接 / 触发源 / 曝光增益"]
B -->|控制单元失败| D["检查 /healthz /status.html / MpcInterfaceIp / 端口 / 机器人状态"]
B -->|机器人报警| E["检查 FANUC 控制柜 / RVBUSTSM / J519 通道 / 安全状态"]
B -->|AI 无结果| F["检查 Ai_Model.ApiUrl / 文件传输目录 / AI 服务日志 / 结果图路径"]
B -->|PLC 不触发| G["检查 OPC UA 地址 / ActiveProfile / StartFlyShotTask 上升沿 / SystemReady"]
B -->|MES 失败| H["检查 EnableMes / ServerIp / ServerPort / TelegramFolder / 报文响应"]
C --> I["查看上位机日志和图片目录"]
D --> I
E --> I
F --> I
G --> I
H --> I
```
### 10.1 相机问题
| 现象 | 排查点 |
| --- | --- |
| 无图像回调 | 相机 IP、本机网卡、相机初始化、回调注册 |
| 图片曝光异常 | 飞拍点位配置中的曝光和增益 |
| 图片保存失败 | `CameraSavePath` 权限、磁盘空间、路径是否存在 |
### 10.2 飞拍控制单元问题
| 现象 | 排查点 |
| --- | --- |
| 上位机无法连接 MPC | `MPC.MpcInterfaceIp` / `MpcInterfacePort` 是否指向控制单元 |
| `/execute_flyshot/` 失败 | 轨迹是否已上传、机器人是否初始化、`method` 是否支持 |
| 机器人不运动 | `/status.html` 状态、FANUC 通道、使能状态、控制柜报警 |
| 轨迹执行后无拍照 | `shot_flags`、`offset_values`、`addrs`、J519 IO 脉冲 |
### 10.3 AI 问题
| 现象 | 排查点 |
| --- | --- |
| AI 请求失败 | `Ai_Model.ApiUrl`、AI 服务是否启动、HTTP 是否可达 |
| AI 找不到图片 | `ai_upload_path`、文件传输服务、远端目录权限 |
| 结果图下载失败 | `ai_result_path`、`AiImagePath`、SFTP/SMB/LOCAL 配置 |
| 结果和图片错位 | 检查 AI 响应中的 `index` 是否和请求一致 |
### 10.4 PLC/MES 问题
| 现象 | 排查点 |
| --- | --- |
| PLC 不触发任务 | `PLC.EnablePlc`、OPC UA 地址、变量映射、上升沿保持时间 |
| PLC 反馈不更新 | 输出变量是否可写、`SystemBusy/SystemReady` 状态 |
| MES 连接失败 | `MesConfig.EnableMes`、IP、端口、防火墙 |
| MES 报文异常 | `TelegramFolder` 下保存的 XML 报文和响应 |
## 11. 交付与飞书整理建议
建议后续整理为飞书文档时采用以下方式：
1. 以本文档 Markdown 作为源稿。
2. 将 Mermaid 流程图渲染为 PNG 或 SVG 图片后插入飞书。
3. 保留接口表、配置表和操作步骤，适合现场培训。
4. 如果需要正式发布版本，可再从 Markdown 生成 Word，再导入飞书云文档。
5. 每次接口、配置或流程变更后，优先更新 Markdown 源稿，再同步飞书版本。
建议拆分飞书目录：
```text
飞拍系统说明
├─ 1. 系统总览
├─ 2. 生产主流程
├─ 3. 上位机使用教程
├─ 4. 飞拍控制单元使用教程
├─ 5. YOLO AI 接口说明
├─ 6. PLC 与 MES 交互
└─ 7. 常见问题排查
```

330
docs/superpowers/plans/2026-06-22-rust-icsp-native-implementation.md Normal file
View File

@@ -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<NativePlanningException>(() => 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_planning"
version = "0.1.0"
edition = "2021"
[lib]
crate-type = ["cdylib"]
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"
```
```xml
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<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>
```
- [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<RuntimeInformation, string> _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(&times, &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<f64>,
coefficients: Vec<[f64; 4]>,
}
impl CubicSpline1D {
pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result<Self, NativePlanningError> { /* ... */ }
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<IcspPlanResult, NativePlanningError> { /* ... */ }
```
- [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_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_planning.dll`
- Linux 需要 `libflyshot_planning.so`
```

271
docs/superpowers/specs/2026-06-22-rust-icsp-native-design.md Normal file
View File

@@ -0,0 +1,271 @@
# Rust ICSP 原生规划核心设计
## 1. 目标
在不改变现有上层业务接口的前提下，使用 Rust 重新实现当前 C# 版本的 ICSP 和 clamped-zero 三次样条算法，并通过稳定的 C ABI 供 .NET 8 调用。
第一阶段必须实现：
- Rust `cdylib` 中的 clamped-zero 三次样条系数求解。
- 样条位置求值和每段一至三阶导数绝对值峰值解析计算。
- ICSP 初始分段时长、迭代缩放、最优结果保存和最终收敛判定。
- C# P/Invoke 适配层。
- Rust 与现有 C# 算法的自动对拍测试。
- Windows x64 和 Linux x64 原生库加载边界。
第一阶段不实现：
- `SelfAdaptIcspPlanner` 的补中点逻辑。
- 轨迹稠密采样、J519 发送采样和触发绑定。
- 正运动学、MoveJoint 和 MovePose。
- FANUC 协议、Socket 和运行时状态机。
- 删除或替换现有 C# ICSP 实现。
## 2. 总体架构
新增原生算法模块和 C# 适配模块：
```text
ControllerClientTrajectoryOrchestrator
|
ICSP 规划接口
/ \
C# ICspPlanner RustIcspPlanner
|
Flyshot.Native.Interop
|
C ABI
|
flyshot_planning
Rust cdylib
```
现有 `ICspPlanner` 保持不变，继续作为行为基准和回退实现。Rust 规划器返回的数据仍由 C# 组装成现有 `PlannedTrajectory`，因此领域层、兼容层和运行时不感知原生对象。
## 3. Rust 模块边界
原生模块建议位于：
```text
native/
flyshot-native-planning/
Cargo.toml
src/
lib.rs
error.rs
ffi.rs
icsp.rs
spline.rs
```
各文件职责：
- `spline.rs`：三对角方程求解、样条系数、位置求值、解析导数峰值。
- `icsp.rs`：纯 Rust ICSP 输入、迭代规划和结果模型。
- `ffi.rs`：C ABI 数据结构、指针校验、结果分配与释放。
- `error.rs`：原生错误分类和 UTF-8 错误信息。
- `lib.rs`：只导出稳定 ABI，不导出内部算法符号。
Rust 内部算法不依赖 .NET 类型、JSON、日志框架、网络或文件系统。
## 4. C ABI 设计
ABI 使用固定宽度整数、C 布局结构和连续 `double` 数组。路点输入采用行优先扁平布局：
```text
[waypoint0_joint0, waypoint0_joint1, ...,
waypoint1_joint0, waypoint1_joint1, ...]
```
主要入口：
```c
int32_t flyshot_icsp_plan(
const double* waypoints,
int32_t waypoint_count,
int32_t dof,
const double* velocity_limits,
const double* acceleration_limits,
const double* jerk_limits,
const FlyshotIcspOptions* options,
FlyshotIcspResult** result);
void flyshot_icsp_result_free(FlyshotIcspResult* result);
const char* flyshot_last_error_message(void);
```
`FlyshotIcspOptions` 包含：
- `threshold`
- `max_iterations`
- `enforce_final_scale`
- `final_scale_tolerance`
`FlyshotIcspResult` 包含：
- `waypoint_times` 及长度
- `segment_durations` 及长度
- `segment_scales` 及长度
- `iterations`
- `threshold`
调用成功返回 `0`。输入非法、数值计算失败、未收敛和内部异常使用不同错误码。错误文本由线程局部存储保存，只保证在当前线程下一次原生调用前有效。
所有结果内存都由 Rust 分配，并且只能通过 `flyshot_icsp_result_free` 释放。C# 不直接释放 Rust 分配的数组。
所有可预期失败必须在 FFI 内转换为错误码，FFI 路径不得依赖 panic 表达业务错误。Debug 和测试构建使用 `catch_unwind` 检测遗漏的 panic；Release 使用 `panic=abort`，此时 `catch_unwind` 不能恢复 panic，因此实现必须通过边界校验避免索引越界、空指针解引用和显式 `unwrap`。任何构建模式都不得让 Rust unwind 穿过 C ABI。
## 5. C# 适配层
新增一个只负责原生互操作的 C# 项目，建议命名为：
```text
src/Flyshot.Native.Planning/
```
包含：
- `NativeIcspMethods`：使用 `LibraryImport` 声明 C ABI。
- `NativeIcspSafeHandle`：封装 Rust 结果指针和释放函数。
- `RustIcspPlanner`：把 `TrajectoryRequest` 转换为连续数组，再把结果转换为 `PlannedTrajectory`。
- `NativePlanningLibraryResolver`：按当前操作系统和体系结构解析原生库。
- `NativePlanningException`：携带原生错误码和错误文本。
原生库逻辑名称固定为 `flyshot_planning`：
- Windows：`flyshot_planning.dll`
- Linux：`libflyshot_planning.so`
C# 适配层负责：
- 检查输入自由度和数组长度。
- 固定数组，避免调用期间被 GC 移动。
- 将 Rust 返回数据复制到托管数组。
- 无论成功或失败都正确释放结果句柄。
- 将原生错误转换为包含诊断信息的 C# 异常。
`RustIcspPlanner` 的构造参数和 `Plan` 行为应与 `ICspPlanner` 对齐，但第一阶段不自动替换现有依赖注入注册。
## 6. 数值一致性
Rust 实现必须逐项复刻当前 C# 行为：
- 初始段时长使用相邻路点的关节空间欧氏距离。
- 使用 clamped-zero 边界条件求三次样条。
- 每轮解析计算各段速度、加速度和 jerk 峰值。
- 分段倍率分别使用一次方、平方根和立方根。
- 收敛指标为 `sum(abs(scale - 1))`。
- 始终保存历史最小收敛指标对应的结果。
- 迭代范围与当前 C# 保持一致，即包含第 `max_iterations` 轮。
- 最终按 `global_scale > 1 + tolerance` 判定是否失败。
第一阶段采用以下对拍容差：
- 路点时间：绝对误差不超过 `1e-9`。
- 段时长：绝对误差不超过 `1e-9`。
- 段缩放：绝对误差不超过 `1e-9`。
- 最终 threshold：绝对误差不超过 `1e-9`。
- 迭代次数：必须完全一致。
如果真实样本因浮点指令或编译器优化无法满足 `1e-9`，必须先定位首个分歧计算步骤，再基于证据调整容差，不直接放宽整个测试。
## 7. 测试策略
遵循 TDD，先增加失败测试，再实现 Rust 和 C# 适配。
测试分四层：
1. Rust 单元测试
- 三次样条边界值。
- 一至三阶导数。
- 解析峰值。
- ICSP 收敛与不收敛。
- 非法指针之外的纯算法输入校验。
2. Rust FFI 测试
- 正常调用和结果释放。
- 空指针、错误维度、非有限数。
- 错误码与错误文本。
- 重复释放由 C# `SafeHandle` 防止，不把未定义行为包装成可接受用法。
3. C# 互操作测试
- 原生库加载。
- 托管数组到 C ABI 的转换。
- 结果复制和释放。
- 原生错误到 C# 异常的转换。
4. C#/Rust 对拍测试
- 仓库中的稳定规划样本。
- 当前 `PlanningCompatibilityTests` 使用的基础输入。
- 多自由度、多路点和极限参数。
- `maxIterations=0` 的未收敛路径。
- 至少一个父目录真实样本固化后的 golden case。
对拍测试必须同时调用现有 `ICspPlanner` 和 `RustIcspPlanner`，不能把预期值只写成 Rust 自身输出。
## 8. 构建与发布
本地构建脚本负责：
- 执行 `cargo build --release`。
- 根据目标平台复制原生库到 C# 测试和发布输出目录。
- Windows x64 构建 Rust `x86_64-pc-windows-msvc`。
- Linux x64 构建 Rust `x86_64-unknown-linux-gnu`。
第一阶段不要求在 Windows 主机上完成 Linux 交叉编译；Linux 原生库可以在 Linux 构建环境生成。仓库必须提供对应命令和 CI/离线构建说明。
发布时原生库与托管程序集放在同一部署目录。现有 Obfuscar 继续处理 `Flyshot.*.dll` 托管程序集，不处理 Rust 原生库。
Rust Release 配置启用：
```toml
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"
```
## 9. 回退与上线边界
第一阶段只提供显式的 Rust 规划器，不改变生产默认算法。完成全部对拍和跨平台验证后，再单独设计配置切换：
```text
Planning:Backend = Managed | Rust
```
原生库缺失、架构不匹配或加载失败时，显式选择 Rust 后应返回清晰错误，不静默回退。静默回退会掩盖部署缺陷，也无法确认现场实际使用的是哪套算法。
保留 C# 实现至少经过一个完整现场验证周期。后续是否删除托管算法，需要独立评审，不属于本设计范围。
## 10. 验收标准
- Rust 单元测试全部通过。
- C# 原生互操作测试全部通过。
- 选定样本的 C#/Rust 对拍全部通过。
- `dotnet test` 和解决方案构建通过。
- Windows x64 实际加载 `.dll` 并完成规划。
- 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_planning.dll` 完成规划。
当前上线边界保持不变：
- 默认生产路径继续使用 C# `ICspPlanner`。
- Rust 规划器只允许显式选择，库缺失、路径不一致或架构不支持时返回明确错误。
- Linux x64 的源码、库名和构建脚本边界已落地，但 `.so` 的实际加载验证必须在
Linux x64 构建环境执行后才能标记完成。

14
docs/use-do-output-selection.md
View File

@@ -1,14 +0,0 @@
# 飞拍 DO / RO 输出选择
`RobotConfig.json` 的 `robot.use_do` 用于选择飞拍输出类型：
- `true`：使用 DO，J519 `writeIoType=2`。
- `false`：使用 RO，J519 `writeIoType=9`。
两种模式都根据 `shot_flags / offset_values / addr` 生成触发事件，保持 `io_keep_cycles` 的脉冲保持与清零行为。`use_do=false` 不再关闭拍照触发。
触发时间轴将输出选择固化到每个事件中；FANUC 运行时根据事件选择写入类型，触发帧和清零帧使用相同类型。未携带输出选择的已有事件默认使用 DO，普通无触发运动保持原行为。
本次不修改配置中的 `addr`，不增加 RO 点位范围校验，现场点位由调用方自行调试。手动 HTTP `SetIo` 仍使用显式 `io_type`。
验证覆盖配置到事件的传递，以及 DO / RO 两种模式下的 J519 脉冲与清零。软件测试不代表末端接线和真实相机触发已完成验收。

7
native/flyshot-native-planning/Cargo.lock generated Normal file
View File

@@ -0,0 +1,7 @@
# This file is automatically @generated by Cargo.
# It is not intended for manual editing.
version = 4
[[package]]
name = "flyshot_planning"
version = "0.1.0"

13
native/flyshot-native-planning/Cargo.toml Normal file
View File

@@ -0,0 +1,13 @@
[package]
name = "flyshot_planning"
version = "0.1.0"
edition = "2021"
[lib]
crate-type = ["cdylib"]
[profile.release]
lto = true
codegen-units = 1
strip = "symbols"
panic = "abort"

33
native/flyshot-native-planning/src/error.rs Normal file
View File

@@ -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 {}

436
native/flyshot-native-planning/src/ffi.rs Normal file
View File

@@ -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<CString> = 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<f64>` 转为由 C ABI 结果持有的裸指针和 i32 长度。
///
/// # 参数
/// - `values`: 需要转移给结果对象的连续数组。
///
/// # 返回值
/// 数组裸指针及其长度；当前输入长度来源于 i32 路点维度，因此不会溢出。
fn leak_f64_array(values: Vec<f64>) -> (*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("有限数"));
}
}

367
native/flyshot-native-planning/src/icsp.rs Normal file
View File

@@ -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<f64>,
/// 历史最优结果的逐段时长。
pub segment_durations: Vec<f64>,
/// 历史最优结果的逐段约束缩放因子。
pub segment_scales: Vec<f64>,
/// 历史最优结果产生时已执行的迭代轮数。
pub iterations: usize,
/// 历史最小 `sum(abs(scale - 1))`。
pub threshold: f64,
}
/// 执行 ICSP 迭代规划并返回历史最优结果。
///
/// # 参数
/// - `input`: 行优先路点、逐关节约束和迭代选项。
///
/// # 返回值
/// 输入合法并通过最终 scale 检查时返回规划结果；否则返回明确错误。
pub fn plan_icsp(input: &IcspInput<'_>) -> Result<IcspPlanResult, NativePlanningError> {
validate_input(input)?;
let mut segment_durations = compute_initial_durations(input)?;
let segment_count = segment_durations.len();
let mut best_result: Option<IcspPlanResult> = 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::<f64>();
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<Vec<f64>, 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<Vec<f64>, 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<Vec<CubicSpline1D>, 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(_)));
}
}

6
native/flyshot-native-planning/src/lib.rs Normal file
View File

@@ -0,0 +1,6 @@
//! Flyshot ICSP 原生规划库的稳定入口。
mod error;
mod ffi;
mod icsp;
mod spline;

277
native/flyshot-native-planning/src/spline.rs Normal file
View File

@@ -0,0 +1,277 @@
//! clamped-zero 三次样条算法。
use crate::error::NativePlanningError;
/// 表示单自由度 clamped-zero 三次样条及其逐段多项式系数。
#[derive(Debug, Clone)]
pub struct CubicSpline1D {
knots: Vec<f64>,
coefficients: Vec<[f64; 4]>,
}
// 位置和逐阶导数求值是第一阶段要求的原生能力，当前 ICSP 热路径只直接使用解析峰值。
#[allow(dead_code)]
impl CubicSpline1D {
/// 使用严格递增时间节点和对应值构造首尾速度为零的三次样条。
///
/// # 参数
/// - `times`: 严格递增且全部有限的时间节点，长度至少为 2。
/// - `values`: 与时间节点一一对应的有限位置值。
///
/// # 返回值
/// 输入合法且三对角方程可解时返回样条，否则返回输入或数值错误。
pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result<Self, NativePlanningError> {
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(&times, &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(&times, &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);
}
}

7
scripts/Publish-Obfuscated.ps1
View File

@@ -427,6 +427,13 @@ function Install-ObfuscatedAssemblies
$_.Name -like 'Flyshot*.pdb' $_.Name -like 'Flyshot*.pdb'
} | } |
Remove-Item -Force Remove-Item -Force
# Rust ICSP 原生库必须随框架依赖包一起交付，否则默认 method=icsp 会在运行时加载失败。
$nativePlanningLibraryPath = Join-Path $PublishedDirectory 'flyshot_planning.dll'
if (-not (Test-Path -LiteralPath $nativePlanningLibraryPath -PathType Leaf))
{
throw "发布目录缺少 Rust 原生规划库 flyshot_planning.dll：$nativePlanningLibraryPath"
}
} }
<# <#

44
src/Flyshot.CSharp.Planning/CSharpIcspPlanningBackend.cs Normal file
View File

@@ -0,0 +1,44 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 将拆分后的 C# ICSP 规划器暴露为可选规划后端。
/// </summary>
public sealed class CSharpIcspPlanningBackend : IPlanningBackend
{
private readonly ICspPlanner _planner;
/// <summary>
/// 初始化 C# ICSP 后端。
/// </summary>
/// <param name="logger">C# ICSP 规划器日志记录器；允许为空。</param>
public CSharpIcspPlanningBackend(ILogger<ICspPlanner>? logger = null)
{
_planner = new ICspPlanner(logger: logger);
}
/// <summary>
/// 获取外部调用 C# ICSP 后端时使用的准确方法名。
/// </summary>
public string MethodName => "icsp-csharp";
/// <summary>
/// 获取 C# ICSP 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.IcspCSharp;
/// <summary>
/// 执行 C# ICSP 规划，并将结果标记为 C# 后端。
/// </summary>
/// <param name="request">包含机器人约束和示教路点的规划请求。</param>
/// <param name="context">规划上下文；普通 C# ICSP 当前不使用该参数。</param>
/// <returns>由 C# ICSP 规划器生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request);
}
}

24
src/Flyshot.CSharp.Planning/CSharpPlanningServiceCollectionExtensions.cs Normal file
View File

@@ -0,0 +1,24 @@
using Flyshot.Core.Planning;
using Microsoft.Extensions.DependencyInjection;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 提供可选 C# 规划算法后端的依赖注入注册入口。
/// </summary>
public static class CSharpPlanningServiceCollectionExtensions
{
/// <summary>
/// 显式注册 C# ICSP 与 self-adapt-icsp 后端。
/// </summary>
/// <param name="services">当前宿主服务集合。</param>
/// <returns>同一服务集合，便于链式调用。</returns>
public static IServiceCollection AddCSharpPlanningBackends(this IServiceCollection services)
{
ArgumentNullException.ThrowIfNull(services);
services.AddSingleton<IPlanningBackend, CSharpIcspPlanningBackend>();
services.AddSingleton<IPlanningBackend, SelfAdaptIcspPlanningBackend>();
return services;
}
}

13
src/Flyshot.CSharp.Planning/Flyshot.CSharp.Planning.csproj Normal file
View File

@@ -0,0 +1,13 @@
<Project Sdk="Microsoft.NET.Sdk">
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
</ItemGroup>
<ItemGroup>
<PackageReference Include="Microsoft.Extensions.DependencyInjection.Abstractions" Version="8.0.2" />
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
</ItemGroup>
</Project>

7
src/Flyshot.Core.Planning/ICSP-算法说明.md → src/Flyshot.CSharp.Planning/ICSP-算法说明.md
View File

@@ -1,6 +1,6 @@
# ICSP 算法说明（`ICspPlanner`） # ICSP 算法说明（`ICspPlanner`）
本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。 本文档用于解释 `Flyshot.CSharp.Planning.ICspPlanner` 当前实现的 **C# ICSP 规划算法**在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。
> 适用范围：本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代（retiming）”** 版本。 > 适用范围：本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代（retiming）”** 版本。
> 重要澄清：`ICspPlanner` 的主要输出是 **时间轴**（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。 > 重要澄清：`ICspPlanner` 的主要输出是 **时间轴**（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。
@@ -186,9 +186,8 @@ globalScale > 1 + \text{finalScaleTolerance}
## 7. 关联实现位置（便于跳转） ## 7. 关联实现位置（便于跳转）
- 算法入口：`src/Flyshot.Core.Planning/ICspPlanner.cs` - 算法入口：`src/Flyshot.CSharp.Planning/ICspPlanner.cs`
- 自适应补点：`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs` - 自适应补点：`src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanner.cs`
- 三次样条实现（clamped-zero + 解析导峰值）：`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs` - 三次样条实现（clamped-zero + 解析导峰值）：`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs`
- 规划层稠密采样：`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs` - 规划层稠密采样：`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs`
- J519 实发重采样：`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs` - J519 实发重采样：`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs`

5
src/Flyshot.Core.Planning/ICspPlanner.cs → src/Flyshot.CSharp.Planning/ICspPlanner.cs
View File

@@ -1,7 +1,8 @@
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging; using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Planning; namespace Flyshot.CSharp.Planning;
/// <summary> /// <summary>
/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。 /// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。
@@ -196,7 +197,7 @@ public sealed class ICspPlanner
waypointTimes: bestWaypointTimes, waypointTimes: bestWaypointTimes,
segmentDurations: bestDurations, segmentDurations: bestDurations,
segmentScales: bestScales, segmentScales: bestScales,
method: PlanningMethod.Icsp, method: PlanningMethod.IcspCSharp,
iterations: bestIterations, iterations: bestIterations,
threshold: bestThreshold); threshold: bestThreshold);
} }

3
src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs → src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanner.cs
View File

@@ -1,7 +1,8 @@
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging; using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Planning; namespace Flyshot.CSharp.Planning;
/// <summary> /// <summary>
/// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。 /// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。

44
src/Flyshot.CSharp.Planning/SelfAdaptIcspPlanningBackend.cs Normal file
View File

@@ -0,0 +1,44 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.CSharp.Planning;
/// <summary>
/// 将 C# self-adapt-icsp 规划器暴露为可选规划后端。
/// </summary>
public sealed class SelfAdaptIcspPlanningBackend : IPlanningBackend
{
private readonly SelfAdaptIcspPlanner _planner;
/// <summary>
/// 初始化 C# self-adapt-icsp 后端。
/// </summary>
/// <param name="logger">self-adapt-icsp 规划器日志记录器；允许为空。</param>
public SelfAdaptIcspPlanningBackend(ILogger<SelfAdaptIcspPlanner>? logger = null)
{
_planner = new SelfAdaptIcspPlanner(logger);
}
/// <summary>
/// 获取外部调用 self-adapt-icsp 后端时使用的准确方法名。
/// </summary>
public string MethodName => "self-adapt-icsp";
/// <summary>
/// 获取 self-adapt-icsp 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.SelfAdaptIcsp;
/// <summary>
/// 执行 C# 自适应 ICSP 规划，按上下文使用配置中的补点次数。
/// </summary>
/// <param name="request">包含机器人约束、示教路点和触发元数据的规划请求。</param>
/// <param name="context">包含 self-adapt-icsp 最大补点次数的规划上下文。</param>
/// <returns>由 self-adapt-icsp 规划器生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request, context.AdaptIcspTryNum);
}
}

2
src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs
View File

@@ -49,7 +49,7 @@ public sealed class ControllerClientCompatRobotCatalog
if (!SupportedRobotModelFileMap.TryGetValue(robotName, out var modelFileName)) if (!SupportedRobotModelFileMap.TryGetValue(robotName, out var modelFileName))
{ {
throw new InvalidOperationException($"不支持的机器人名称 Unsupported robot name: {robotName}"); throw new InvalidOperationException($"Unsupported robot name: {robotName}");
} }
var modelPath = ResolveModelPath(modelFileName); var modelPath = ResolveModelPath(modelFileName);

15
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
View File

@@ -651,7 +651,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <summary> /// <summary>
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。 /// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
/// 规划倍率与当前运行速度倍率相乘后用于生成轨迹，使直接移动和飞拍预移动采用相同限速规则。
/// </summary> /// </summary>
/// <param name="robot">当前机器人模型。</param> /// <param name="robot">当前机器人模型。</param>
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param> /// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
@@ -663,21 +662,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions)); EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));
var speedRatio = _runtime.GetSnapshot().SpeedRatio; var speedRatio = _runtime.GetSnapshot().SpeedRatio;
double planningSpeedScale; var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
lock (_stateLock)
{
planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
}
// 在生成阶段统一缩放速度、加速度和跃度限制，发送阶段仍保持固定伺服周期。
var effectivePlanningSpeedScale = planningSpeedScale * speedRatio;
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, effectivePlanningSpeedScale, _logger);
_logger?.LogInformation( _logger?.LogInformation(
"{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划倍率={PlanningSpeedScale}, 有效规划倍率={EffectivePlanningSpeedScale}, 规划时长={Duration}s, 采样点数={SampleCount}", "{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
operationName, operationName,
speedRatio, speedRatio,
planningSpeedScale,
effectivePlanningSpeedScale,
moveResult.Duration.TotalSeconds, moveResult.Duration.TotalSeconds,
moveResult.DenseJointTrajectory?.Count ?? 0); moveResult.DenseJointTrajectory?.Count ?? 0);

3
src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs
View File

@@ -1,4 +1,6 @@
using Flyshot.Core.Config; using Flyshot.Core.Config;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;
using Flyshot.Runtime.Common; using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc; using Flyshot.Runtime.Fanuc;
using Microsoft.Extensions.Configuration; using Microsoft.Extensions.Configuration;
@@ -31,6 +33,7 @@ public static class ControllerClientCompatServiceCollectionExtensions
services.AddSingleton<RobotModelLoader>(); services.AddSingleton<RobotModelLoader>();
services.AddSingleton<RobotConfigLoader>(); services.AddSingleton<RobotConfigLoader>();
services.AddSingleton<ControllerClientCompatRobotCatalog>(); services.AddSingleton<ControllerClientCompatRobotCatalog>();
services.AddSingleton<IPlanningBackend, RustIcspPlanningBackend>();
services.AddSingleton<ControllerClientTrajectoryOrchestrator>(); services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
services.AddSingleton<FlyshotTrajectoryArtifactWriter>(); services.AddSingleton<FlyshotTrajectoryArtifactWriter>();
services.AddSingleton<JsonFlyshotTrajectoryStore>(); services.AddSingleton<JsonFlyshotTrajectoryStore>();

104
src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs
View File

@@ -3,6 +3,7 @@ using Flyshot.Core.Domain;
using Flyshot.Core.Planning; using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling; using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering; using Flyshot.Core.Triggering;
using Flyshot.Native.Planning;
using Microsoft.Extensions.Logging; using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat; namespace Flyshot.ControllerClientCompat;
@@ -28,8 +29,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary> /// </summary>
private const double SmoothStartStopBlend = 0.60; private const double SmoothStartStopBlend = 0.60;
private readonly ICspPlanner _icspPlanner; private readonly IReadOnlyDictionary<string, IPlanningBackend> _planningBackends;
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver()); private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal); private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger; private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
@@ -37,15 +37,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <summary> /// <summary>
/// 初始化轨迹编排器。 /// 初始化轨迹编排器。
/// </summary> /// </summary>
/// <param name="planningBackends">显式注册的规划后端；为空时仅启用默认 Rust icsp 后端。</param>
/// <param name="logger">日志记录器；允许 null。</param> /// <param name="logger">日志记录器；允许 null。</param>
/// <param name="loggerFactory">日志工厂；允许 null。</param> /// <param name="loggerFactory">日志工厂；允许 null。</param>
public ControllerClientTrajectoryOrchestrator( public ControllerClientTrajectoryOrchestrator(
IEnumerable<IPlanningBackend>? planningBackends = null,
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null, ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
ILoggerFactory? loggerFactory = null) ILoggerFactory? loggerFactory = null)
{ {
_logger = logger; _logger = logger;
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>()); _planningBackends = BuildPlanningBackendMap(planningBackends);
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
} }
/// <summary> /// <summary>
@@ -81,14 +82,15 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: Enumerable.Repeat(0, waypoints.Count), offsetValues: Enumerable.Repeat(0, waypoints.Count),
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>())); addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
var method = ParseOrdinaryMethod(options.Method); var methodName = ParseOrdinaryMethod(options.Method);
var backend = ResolvePlanningBackend(methodName);
var request = new TrajectoryRequest( var request = new TrajectoryRequest(
robot: planningRobot, robot: planningRobot,
program: program, program: program,
method: method, method: backend.ResultMethod,
saveTrajectoryArtifacts: options.SaveTrajectory); saveTrajectoryArtifacts: options.SaveTrajectory);
var plannedTrajectory = PlanByMethod(request, method); var plannedTrajectory = PlanByBackend(request, backend);
var executionTrajectory = plannedTrajectory; var executionTrajectory = plannedTrajectory;
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false); var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>()); var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
@@ -152,7 +154,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: uploaded.OffsetValues, offsetValues: uploaded.OffsetValues,
addressGroups: uploaded.AddressGroups); addressGroups: uploaded.AddressGroups);
var method = ParseFlyshotMethod(options.Method); var methodName = ParseFlyshotMethod(options.Method);
var backend = ResolvePlanningBackend(methodName);
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale); var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{ {
@@ -165,12 +168,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
var request = new TrajectoryRequest( var request = new TrajectoryRequest(
robot: planningRobot, robot: planningRobot,
program: program, program: program,
method: method, method: backend.ResultMethod,
moveToStart: options.MoveToStart, moveToStart: options.MoveToStart,
saveTrajectoryArtifacts: options.SaveTrajectory, saveTrajectoryArtifacts: options.SaveTrajectory,
useCache: options.UseCache); useCache: options.UseCache);
var plannedTrajectory = PlanByMethod(request, method, settings); var plannedTrajectory = PlanByBackend(request, backend, settings);
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings); var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false); var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
var shotTimeline = _shotTimelineBuilder.Build( var shotTimeline = _shotTimelineBuilder.Build(
@@ -203,13 +206,15 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary> /// </summary>
/// <param name="method">旧 SDK 传入的方法名。</param> /// <param name="method">旧 SDK 传入的方法名。</param>
/// <returns>领域层规划方法。</returns> /// <returns>领域层规划方法。</returns>
private static PlanningMethod ParseOrdinaryMethod(string method) private static string ParseOrdinaryMethod(string method)
{ {
var normalized = NormalizeMethod(method); var normalized = NormalizeMethod(method);
return normalized switch return normalized switch
{ {
"icsp" => PlanningMethod.Icsp, "icsp" => normalized,
"doubles" => PlanningMethod.Doubles, "icsp-csharp" => normalized,
"icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
"doubles" => normalized,
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method)) _ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
}; };
} }
@@ -219,33 +224,80 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// </summary> /// </summary>
/// <param name="method">旧 SDK 传入的方法名。</param> /// <param name="method">旧 SDK 传入的方法名。</param>
/// <returns>领域层规划方法。</returns> /// <returns>领域层规划方法。</returns>
private static PlanningMethod ParseFlyshotMethod(string method) private static string ParseFlyshotMethod(string method)
{ {
var normalized = NormalizeMethod(method); var normalized = NormalizeMethod(method);
return normalized switch return normalized switch
{ {
"icsp" => PlanningMethod.Icsp, "icsp" => normalized,
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp, "icsp-csharp" => normalized,
"doubles" => PlanningMethod.Doubles, "icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
"self-adapt-icsp" => normalized,
"doubles" => normalized,
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method)) _ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
}; };
} }
/// <summary> /// <summary>
/// 按领域枚举分派到当前已经落地的规划器。 /// 使用显式注册的规划后端执行规划。
/// </summary> /// </summary>
/// <param name="request">规划请求。</param> /// <param name="request">规划请求。</param>
/// <param name="method">规划方法。</param> /// <param name="backend">已解析的规划后端。</param>
/// <param name="settings">当前机器人兼容配置；为空时使用默认补点次数。</param>
/// <returns>规划轨迹。</returns> /// <returns>规划轨迹。</returns>
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null) private static PlannedTrajectory PlanByBackend(
TrajectoryRequest request,
IPlanningBackend backend,
CompatibilityRobotSettings? settings = null)
{ {
return method switch var context = new PlanningBackendContext(settings?.AdaptIcspTryNum ?? 5);
return backend.Plan(request, context);
}
/// <summary>
/// 从已注册后端中解析 method 对应的规划实现。
/// </summary>
/// <param name="methodName">已经归一化的外部 method 名称。</param>
/// <returns>可执行该 method 的规划后端。</returns>
private IPlanningBackend ResolvePlanningBackend(string methodName)
{
if (methodName == "doubles")
{ {
PlanningMethod.Icsp => _icspPlanner.Plan(request), throw new NotSupportedException("doubles 轨迹规划尚未落地。");
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5), }
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。") if (_planningBackends.TryGetValue(methodName, out var backend))
}; {
return backend;
}
throw new NotSupportedException($"轨迹规划方法 {methodName} 未注册对应后端。");
}
/// <summary>
/// 构造规划后端查找表，未显式注册时只启用默认 Rust icsp 后端。
/// </summary>
/// <param name="planningBackends">调用方显式注册的规划后端集合。</param>
/// <returns>按 method 名称索引的后端表。</returns>
private static IReadOnlyDictionary<string, IPlanningBackend> BuildPlanningBackendMap(IEnumerable<IPlanningBackend>? planningBackends)
{
var backends = planningBackends?.ToArray() ?? [];
if (backends.Length == 0)
{
backends = [new RustIcspPlanningBackend()];
}
var map = new Dictionary<string, IPlanningBackend>(StringComparer.Ordinal);
foreach (var backend in backends)
{
var methodName = NormalizeMethod(backend.MethodName);
if (!map.TryAdd(methodName, backend))
{
throw new InvalidOperationException($"规划后端 method={methodName} 被重复注册。");
}
}
return map;
} }
/// <summary> /// <summary>

1
src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj
View File

@@ -5,6 +5,7 @@
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" /> <ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" /> <ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" /> <ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
<ProjectReference Include="..\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" /> <ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" /> <ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
</ItemGroup> </ItemGroup>

2
src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs
View File

@@ -9,7 +9,7 @@ public sealed class FlyshotExecutionOptions
/// 初始化飞拍轨迹执行参数。 /// 初始化飞拍轨迹执行参数。
/// </summary> /// </summary>
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param> /// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
/// <param name="method">轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param> /// <param name="method">轨迹生成方法，支持默认 Rust `icsp`、可选 C# `icsp-csharp`、`self-adapt-icsp` 或 `doubles`。</param>
/// <param name="saveTrajectory">是否保存轨迹信息。</param> /// <param name="saveTrajectory">是否保存轨迹信息。</param>
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param> /// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param> /// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>

2
src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs
View File

@@ -8,7 +8,7 @@ public sealed class TrajectoryExecutionOptions
/// <summary> /// <summary>
/// 初始化普通轨迹执行参数。 /// 初始化普通轨迹执行参数。
/// </summary> /// </summary>
/// <param name="method">轨迹生成方法，支持 `icsp` 或 `doubles`。</param> /// <param name="method">轨迹生成方法，支持默认 Rust `icsp`、可选 C# `icsp-csharp` 或 `doubles`。</param>
/// <param name="saveTrajectory">是否保存轨迹信息。</param> /// <param name="saveTrajectory">是否保存轨迹信息。</param>
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false) public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
{ {

2
src/Flyshot.Core.Config/RobotConfigLoader.cs
View File

@@ -81,7 +81,7 @@ public sealed class CompatibilityRobotSettings
} }
/// <summary> /// <summary>
/// 获取飞拍同步输出类型选择：true 使用 DO，false 使用 RO；两者都启用触发。 /// 获取是否启用伺服同步 IO。
/// </summary> /// </summary>
public bool UseDo { get; } public bool UseDo { get; }

7
src/Flyshot.Core.Domain/TrajectoryRequest.cs
View File

@@ -70,10 +70,15 @@ public sealed class TrajectoryRequest
public enum PlanningMethod public enum PlanningMethod
{ {
/// <summary> /// <summary>
/// Uses the cubic-spline ICSP planner. /// 使用正式 icsp 后端；当前默认实现为 Rust 原生 ICSP。
/// </summary> /// </summary>
Icsp, Icsp,
/// <summary>
/// 使用拆分到可选程序集中的 C# ICSP 规划器。
/// </summary>
IcspCSharp,
/// <summary> /// <summary>
/// Uses the adaptive ICSP planner that may insert additional points. /// Uses the adaptive ICSP planner that may insert additional points.
/// </summary> /// </summary>

21
src/Flyshot.Core.Domain/TrajectoryResult.cs
View File

@@ -174,28 +174,20 @@ public sealed class TrajectoryResult
} }
/// <summary> /// <summary>
/// 表示运行时注入伺服时间轴的 DO 或 RO 输出触发事件。 /// Describes a trigger event that the runtime must inject into the servo timeline.
/// </summary> /// </summary>
public sealed class TrajectoryDoEvent public sealed class TrajectoryDoEvent
{ {
/// <summary> /// <summary>
/// 初始化经过校验的伺服同步输出触发事件。 /// Initializes a validated runtime trigger event.
/// </summary> /// </summary>
/// <param name="waypointIndex">请求拍照的原始示教点索引，从零开始。</param>
/// <param name="triggerTime">离散化之前的理论触发时间，单位秒，必须非负。</param>
/// <param name="offsetCycles">相对于示教点的触发偏移周期数。</param>
/// <param name="holdCycles">输出保持的伺服周期数，必须非负。</param>
/// <param name="addressGroup">要输出的 IO 点位组，不允许为空引用。</param>
/// <param name="referenceJointsDegrees">用于选择最近发送采样点的关节角，单位度；允许 null。</param>
/// <param name="useDo">true 使用 DO，false 使用 RO；默认 true 保留已有事件的 DO 行为。</param>
public TrajectoryDoEvent( public TrajectoryDoEvent(
int waypointIndex, int waypointIndex,
double triggerTime, double triggerTime,
int offsetCycles, int offsetCycles,
int holdCycles, int holdCycles,
IoAddressGroup addressGroup, IoAddressGroup addressGroup,
IEnumerable<double>? referenceJointsDegrees = null, IEnumerable<double>? referenceJointsDegrees = null)
bool useDo = true)
{ {
if (waypointIndex < 0) if (waypointIndex < 0)
{ {
@@ -218,7 +210,6 @@ public sealed class TrajectoryDoEvent
HoldCycles = holdCycles; HoldCycles = holdCycles;
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup)); AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
ReferenceJointsDegrees = referenceJointsDegrees?.ToArray(); ReferenceJointsDegrees = referenceJointsDegrees?.ToArray();
UseDo = useDo;
} }
/// <summary> /// <summary>
@@ -257,12 +248,6 @@ public sealed class TrajectoryDoEvent
/// </summary> /// </summary>
[JsonPropertyName("referenceJointsDegrees")] [JsonPropertyName("referenceJointsDegrees")]
public IReadOnlyList<double>? ReferenceJointsDegrees { get; } public IReadOnlyList<double>? ReferenceJointsDegrees { get; }
/// <summary>
/// 获取飞拍输出选择：true 为 DO，false 为 RO；仅供运行时使用。
/// </summary>
[JsonIgnore]
public bool UseDo { get; }
} }
/// <summary> /// <summary>

27
src/Flyshot.Core.Planning/IPlanningBackend.cs Normal file
View File

@@ -0,0 +1,27 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning;
/// <summary>
/// 定义可由兼容层按 method 字符串分派的轨迹规划后端。
/// </summary>
public interface IPlanningBackend
{
/// <summary>
/// 获取外部 HTTP/ControllerClient method 参数使用的准确方法名。
/// </summary>
string MethodName { get; }
/// <summary>
/// 获取该后端成功规划后写入结果对象的领域规划方法。
/// </summary>
PlanningMethod ResultMethod { get; }
/// <summary>
/// 执行轨迹规划。
/// </summary>
/// <param name="request">包含机器人约束、轨迹程序和调用标志的规划请求。</param>
/// <param name="context">兼容层传入的规划上下文参数。</param>
/// <returns>规划后的轨迹时间轴、路点和收敛信息。</returns>
PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context);
}

21
src/Flyshot.Core.Planning/PlanningBackendContext.cs Normal file
View File

@@ -0,0 +1,21 @@
namespace Flyshot.Core.Planning;
/// <summary>
/// 表示规划后端执行时由兼容层补充的运行参数。
/// </summary>
public sealed class PlanningBackendContext
{
/// <summary>
/// 初始化规划后端上下文。
/// </summary>
/// <param name="adaptIcspTryNum">self-adapt-icsp 允许的最大补点重试次数。</param>
public PlanningBackendContext(int adaptIcspTryNum = 5)
{
AdaptIcspTryNum = adaptIcspTryNum;
}
/// <summary>
/// 获取 self-adapt-icsp 允许的最大补点重试次数。
/// </summary>
public int AdaptIcspTryNum { get; }
}

24
src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs
View File

@@ -12,7 +12,7 @@ namespace Flyshot.Core.Triggering;
/// 本构建器只负责“时间轴映射”和“事件打包”，不负责轨迹规划与运行时发送。 /// 本构建器只负责“时间轴映射”和“事件打包”，不负责轨迹规划与运行时发送。
/// 其中： /// 其中：
/// - ShotEvent 用于诊断、导出和日志观察； /// - ShotEvent 用于诊断、导出和日志观察；
/// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 或 RO 脉冲。 /// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 脉冲。
/// </remarks> /// </remarks>
public sealed class ShotTimelineBuilder public sealed class ShotTimelineBuilder
{ {
@@ -43,7 +43,7 @@ public sealed class ShotTimelineBuilder
/// <param name="trajectory">规划后的轨迹（含补中点信息和机器人配置）。</param> /// <param name="trajectory">规划后的轨迹（含补中点信息和机器人配置）。</param>
/// <param name="holdCycles">IO 保持周期数（对应原系统的 io_keep_cycles）。</param> /// <param name="holdCycles">IO 保持周期数（对应原系统的 io_keep_cycles）。</param>
/// <param name="samplePeriod">稠密采样周期，用于离散化 sample_index 和 sample_time。</param> /// <param name="samplePeriod">稠密采样周期，用于离散化 sample_index 和 sample_time。</param>
/// <param name="useDo">输出类型选择：true 使用 DO，false 使用 RO；两者都生成触发事件。</param> /// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns> /// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
/// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception> /// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception>
/// <exception cref="ArgumentOutOfRangeException"> /// <exception cref="ArgumentOutOfRangeException">
@@ -101,15 +101,17 @@ public sealed class ShotTimelineBuilder
sampleTime: sampleTime, sampleTime: sampleTime,
addressGroup: addressGroup)); addressGroup: addressGroup));
// use_do 只选择输出类型；RO 模式也必须保留拍照脉冲与保持周期。 if (useDo)
triggerTimeline.Add(new TrajectoryDoEvent( {
waypointIndex: i, // use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。
triggerTime: triggerTime, triggerTimeline.Add(new TrajectoryDoEvent(
offsetCycles: program.OffsetValues[i], waypointIndex: i,
holdCycles: holdCycles, triggerTime: triggerTime,
addressGroup: addressGroup, offsetCycles: program.OffsetValues[i],
referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI), holdCycles: holdCycles,
useDo: useDo)); addressGroup: addressGroup,
referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI)));
}
} }
_logger?.LogInformation( _logger?.LogInformation(

15
src/Flyshot.Native.Planning/Flyshot.Native.Planning.csproj Normal file
View File

@@ -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>

77
src/Flyshot.Native.Planning/NativeIcspInteropModels.cs Normal file
View File

@@ -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;
}

130
src/Flyshot.Native.Planning/NativeIcspMethods.cs Normal file
View File

@@ -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;
}
}

24
src/Flyshot.Native.Planning/NativeIcspPlannerOptions.cs Normal file
View File

@@ -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);
}

37
src/Flyshot.Native.Planning/NativeIcspSafeHandle.cs Normal file
View File

@@ -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;
}
}

24
src/Flyshot.Native.Planning/NativePlanningException.cs Normal file
View File

@@ -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; }
}

104
src/Flyshot.Native.Planning/NativePlanningLibraryResolver.cs Normal file
View File

@@ -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_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_planning.dll",
"Linux" => "libflyshot_planning.so",
_ => throw new NativePlanningException(
$"Rust 原生规划库不支持当前操作系统：{platform.OperatingSystem}。",
errorCode: -1003)
};
return Path.Combine(AppContext.BaseDirectory, fileName);
}
}

220
src/Flyshot.Native.Planning/RustIcspPlanner.cs Normal file
View File

@@ -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.Icsp,
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";
}
}

43
src/Flyshot.Native.Planning/RustIcspPlanningBackend.cs Normal file
View File

@@ -0,0 +1,43 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Native.Planning;
/// <summary>
/// 将 Rust ICSP 规划器暴露为默认正式 icsp 后端。
/// </summary>
public sealed class RustIcspPlanningBackend : IPlanningBackend
{
private readonly RustIcspPlanner _planner;
/// <summary>
/// 初始化 Rust ICSP 后端。
/// </summary>
/// <param name="planner">可选的 Rust ICSP 规划器实例；为空时使用默认原生库解析规则。</param>
public RustIcspPlanningBackend(RustIcspPlanner? planner = null)
{
_planner = planner ?? new RustIcspPlanner();
}
/// <summary>
/// 获取外部调用 Rust ICSP 后端时使用的正式方法名。
/// </summary>
public string MethodName => "icsp";
/// <summary>
/// 获取 Rust ICSP 后端写入结果对象的规划方法。
/// </summary>
public PlanningMethod ResultMethod => PlanningMethod.Icsp;
/// <summary>
/// 执行 Rust ICSP 规划。
/// </summary>
/// <param name="request">包含机器人约束和示教路点的规划请求。</param>
/// <param name="context">规划上下文；Rust ICSP 当前不使用该参数。</param>
/// <returns>由 Rust 原生库生成的轨迹结果。</returns>
public PlannedTrajectory Plan(TrajectoryRequest request, PlanningBackendContext context)
{
ArgumentNullException.ThrowIfNull(context);
return _planner.Plan(request);
}
}

74
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
View File

@@ -39,49 +39,23 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
/// 初始化 FANUC 控制器运行时。 /// 初始化 FANUC 控制器运行时。
/// </summary> /// </summary>
/// <param name="logger">运行时日志记录器。</param> /// <param name="logger">运行时日志记录器。</param>
/// <param name="commandLogger">FANUC 10012 命令通道客户端日志记录器。</param>
/// <param name="stateLogger">FANUC 10010 状态通道客户端日志记录器。</param>
/// <param name="j519Logger">J519 客户端日志记录器。</param>
public FanucControllerRuntime(
ILogger<FanucControllerRuntime> logger,
ILogger<FanucCommandClient> commandLogger,
ILogger<FanucStateClient> stateLogger,
ILogger<FanucJ519Client> j519Logger)
{
ArgumentNullException.ThrowIfNull(logger);
ArgumentNullException.ThrowIfNull(commandLogger);
ArgumentNullException.ThrowIfNull(stateLogger);
ArgumentNullException.ThrowIfNull(j519Logger);
_commandClient = new FanucCommandClient(commandLogger);
_stateClient = new FanucStateClient(new FanucStateClientOptions(), stateLogger);
_j519Client = new FanucJ519Client(j519Logger);
_logger = logger;
}
/// <summary>
/// 初始化 FANUC 控制器运行时，并仅显式传入运行时与 J519 日志记录器。
/// </summary>
/// <param name="logger">运行时日志记录器。</param>
/// <param name="j519Logger">J519 客户端日志记录器。</param> /// <param name="j519Logger">J519 客户端日志记录器。</param>
public FanucControllerRuntime(ILogger<FanucControllerRuntime> logger, ILogger<FanucJ519Client> j519Logger) public FanucControllerRuntime(ILogger<FanucControllerRuntime> logger, ILogger<FanucJ519Client> j519Logger)
: this(
logger,
NullLogger<FanucCommandClient>.Instance,
NullLogger<FanucStateClient>.Instance,
j519Logger)
{ {
ArgumentNullException.ThrowIfNull(logger);
ArgumentNullException.ThrowIfNull(j519Logger);
_commandClient = new FanucCommandClient();
_stateClient = new FanucStateClient();
_j519Client = new FanucJ519Client(j519Logger);
_logger = logger;
} }
/// <summary> /// <summary>
/// 初始化一份无日志的 FANUC 控制器运行时，供离线测试或手工构造场景使用。 /// 初始化一份无日志的 FANUC 控制器运行时，供离线测试或手工构造场景使用。
/// </summary> /// </summary>
public FanucControllerRuntime() public FanucControllerRuntime()
: this( : this(NullLogger<FanucControllerRuntime>.Instance, NullLogger<FanucJ519Client>.Instance)
NullLogger<FanucControllerRuntime>.Instance,
NullLogger<FanucCommandClient>.Instance,
NullLogger<FanucStateClient>.Instance,
NullLogger<FanucJ519Client>.Instance)
{ {
} }
@@ -595,10 +569,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken) private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
{ {
var triggers = result.TriggerTimeline; var triggers = result.TriggerTimeline;
// 输出类型随触发事件固化，确保脉冲与清零帧都写入同一路 DO/RO。
byte writeIoType = triggers.FirstOrDefault()?.UseDo == false
? (byte)FanucIoTypes.RobotOutput
: (byte)FanucIoTypes.DigitalOutput;
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds; var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
var preparedExecution = result.PreparedFlyshotExecution; var preparedExecution = result.PreparedFlyshotExecution;
var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds; var durationSeconds = preparedExecution?.FinalDurationSeconds ?? result.Duration.TotalSeconds;
@@ -714,7 +684,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
var command = new FanucJ519Command( var command = new FanucJ519Command(
sequence: 0, sequence: 0,
targetJoints: sample.JointsDegrees, targetJoints: sample.JointsDegrees,
writeIoType: writeIoType, writeIoType: 2,
writeIoIndex: 1, writeIoIndex: 1,
writeIoMask: ioMask, writeIoMask: ioMask,
writeIoValue: ioValue); writeIoValue: ioValue);
@@ -767,20 +737,18 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
sample.TrajectoryTime); sample.TrajectoryTime);
} }
} }
//TODO 不进行结束,一致发送最后一帧 if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0)
{
// if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0) // 正常结束命令流时追加独立结束帧，避免把最后一个真实轨迹采样点和结束语义混在一起。
// { commands.Add(new FanucJ519Command(
// // 正常结束命令流时追加独立结束帧，避免把最后一个真实轨迹采样点和结束语义混在一起。 sequence: 0,
// commands.Add(new FanucJ519Command( targetJoints: commands[^1].TargetJoints,
// sequence: 0, lastData: 1,
// targetJoints: commands[^1].TargetJoints, writeIoType: 2,
// lastData: 1, writeIoIndex: 1,
// writeIoType: 2, writeIoMask: 0,
// writeIoIndex: 1, writeIoValue: 0));
// writeIoMask: 0, }
// writeIoValue: 0));
// }
TryWriteDenseSendArtifacts(result, outputDir, sentJointRows, sentTimingRows, sentJerkRows, triggerBindings); TryWriteDenseSendArtifacts(result, outputDir, sentJointRows, sentTimingRows, sentJerkRows, triggerBindings);
// 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。 // 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。

4
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
View File

@@ -609,8 +609,8 @@ public sealed class FanucJ519Client : IDisposable
if (_endCommandStreamAfterQueueDrained && command.LastData == 1) if (_endCommandStreamAfterQueueDrained && command.LastData == 1)
{ {
// LastData 是命令流的正常结束语义；结束后保留状态接收，但不继续回发保姿包。 // LastData 是命令流的正常结束语义；结束后保留状态接收，但不继续回发保姿包。
// _monitorOnlyAfterLastData = true; _monitorOnlyAfterLastData = true;
// _motionStarted = false; _motionStarted = false;
CompleteCommandQueueLocked(); CompleteCommandQueueLocked();
} }
else if (willDrainQueue && _commandQueue is { Count: 0 }) else if (willDrainQueue && _commandQueue is { Count: 0 })

3
src/Flyshot.Server.Host/Flyshot.Server.Host.csproj
View File

@@ -16,6 +16,9 @@
<Content Include="..\..\NLog.config" Link="NLog.config" CopyToOutputDirectory="PreserveNewest" /> <Content Include="..\..\NLog.config" Link="NLog.config" CopyToOutputDirectory="PreserveNewest" />
<!-- 运行时配置根目录固定为输出目录 Config，调试和发布都复制仓库内固化配置。 --> <!-- 运行时配置根目录固定为输出目录 Config，调试和发布都复制仓库内固化配置。 -->
<Content Include="..\..\Config\**\*" Link="Config\%(RecursiveDir)%(Filename)%(Extension)" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" /> <Content Include="..\..\Config\**\*" Link="Config\%(RecursiveDir)%(Filename)%(Extension)" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
<!-- Rust ICSP 原生规划库：从 src/Libs 固定复制到运行与发布目录，供默认 method=icsp 加载。 -->
<Content Include="..\Libs\flyshot_planning.dll" Link="flyshot_planning.dll" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" Condition="Exists('..\Libs\flyshot_planning.dll')" />
<Content Include="..\Libs\libflyshot_planning.so" Link="libflyshot_planning.so" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" Condition="Exists('..\Libs\libflyshot_planning.so')" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>

4
src/Flyshot.Server.Host/appsettings.json
View File

@@ -8,8 +8,8 @@
"ControllerClientCompat": { "ControllerClientCompat": {
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0", "ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0",
"AutoInit": { "AutoInit": {
"RobotName": "FANUC_LR_Mate_200iD_7L", "RobotName": "LR_Mate_200iD",
"RobotIp": "192.168.10.11" "RobotIp": "127.0.0.1"
} }
}, },
"Swagger": { "Swagger": {

9
src/Flyshot.Server.Host/wwwroot/debug.html
View File

@@ -46,6 +46,7 @@
规划方法 规划方法
<select id="offline-planning-method"> <select id="offline-planning-method">
<option value="icsp">icsp</option> <option value="icsp">icsp</option>
<option value="icsp-csharp">icsp-csharp</option>
<option value="self-adapt-icsp">self-adapt-icsp</option> <option value="self-adapt-icsp">self-adapt-icsp</option>
</select> </select>
</label> </label>
@@ -214,7 +215,7 @@
"description": "执行普通关节轨迹。该接口兼容两种历史请求体形态：一是对象形态 `{ waypoints, method, save_traj }`，二是直接传纯数组路点；查询参数里的 `method` 和 `save_traj` 仍可覆盖请求体值。", "description": "执行普通关节轨迹。该接口兼容两种历史请求体形态：一是对象形态 `{ waypoints, method, save_traj }`，二是直接传纯数组路点；查询参数里的 `method` 和 `save_traj` 仍可覆盖请求体值。",
"requestBodyFields": [ "requestBodyFields": [
{ "name": "waypoints", "type": "array<array<number>> | array<object>", "required": true, "description": "历史兼容对象形态中的轨迹路点集合；每个路点既可直接是关节数组，也可写成 `{ joints: [...] }`。" }, { "name": "waypoints", "type": "array<array<number>> | array<object>", "required": true, "description": "历史兼容对象形态中的轨迹路点集合；每个路点既可直接是关节数组，也可写成 `{ joints: [...] }`。" },
{ "name": "method", "type": "string", "required": false, "description": "对象形态中的轨迹规划方法，默认 `icsp`。" }, { "name": "method", "type": "string", "required": false, "description": "对象形态中的轨迹规划方法，默认 `icsp`；`icsp-csharp` 需要显式注册 C# 后端。" },
{ "name": "save_traj", "type": "boolean", "required": false, "description": "对象形态中的是否保存轨迹标记，默认 `false`。" }, { "name": "save_traj", "type": "boolean", "required": false, "description": "对象形态中的是否保存轨迹标记，默认 `false`。" },
{ "name": "根数组", "type": "array<array<number>> | array<object>", "required": true, "description": "纯数组形态时，请直接提交路点数组本身，而不是对象包裹层。" }, { "name": "根数组", "type": "array<array<number>> | array<object>", "required": true, "description": "纯数组形态时，请直接提交路点数组本身，而不是对象包裹层。" },
{ "name": "根数组[].joints", "type": "array<number>", "required": false, "description": "当路点元素是对象形态时，兼容层会读取其中的 `joints` 数组。" } { "name": "根数组[].joints", "type": "array<number>", "required": false, "description": "当路点元素是对象形态时，兼容层会读取其中的 `joints` 数组。" }
@@ -237,7 +238,7 @@
"requestBodyFields": [ "requestBodyFields": [
{ "name": "name", "type": "string", "required": true, "description": "要执行的飞拍轨迹名称。" }, { "name": "name", "type": "string", "required": true, "description": "要执行的飞拍轨迹名称。" },
{ "name": "move_to_start", "type": "boolean", "required": false, "description": "是否先用临时 PTP 轨迹移动到飞拍起点，默认 `true`。" }, { "name": "move_to_start", "type": "boolean", "required": false, "description": "是否先用临时 PTP 轨迹移动到飞拍起点，默认 `true`。" },
{ "name": "method", "type": "string", "required": false, "description": "飞拍轨迹生成方法，默认 `icsp`。" }, { "name": "method", "type": "string", "required": false, "description": "飞拍轨迹生成方法，默认 `icsp`；`icsp-csharp` 和 `self-adapt-icsp` 需要显式注册 C# 后端。" },
{ "name": "save_traj", "type": "boolean", "required": false, "description": "是否保存轨迹信息，默认 `true`。" }, { "name": "save_traj", "type": "boolean", "required": false, "description": "是否保存轨迹信息，默认 `true`。" },
{ "name": "use_cache", "type": "boolean", "required": false, "description": "是否优先复用已缓存的轨迹结果，默认 `true`。" }, { "name": "use_cache", "type": "boolean", "required": false, "description": "是否优先复用已缓存的轨迹结果，默认 `true`。" },
{ "name": "wait", "type": "boolean", "required": false, "description": "是否等待整条飞拍轨迹执行完成后再返回，默认 `true`。" } { "name": "wait", "type": "boolean", "required": false, "description": "是否等待整条飞拍轨迹执行完成后再返回，默认 `true`。" }
@@ -248,7 +249,7 @@
"description": "仅要求机器人模型已加载；完成规划、触发绑定、最终 8ms 序列生成和文件导出，并返回完整规划结果。", "description": "仅要求机器人模型已加载；完成规划、触发绑定、最终 8ms 序列生成和文件导出，并返回完整规划结果。",
"requestBodyFields": [ "requestBodyFields": [
{ "name": "name", "type": "string", "required": true, "description": "目标飞拍轨迹名称。" }, { "name": "name", "type": "string", "required": true, "description": "目标飞拍轨迹名称。" },
{ "name": "method", "type": "string", "required": false, "description": "轨迹规划方法，默认 `icsp`。" } { "name": "method", "type": "string", "required": false, "description": "轨迹规划方法，默认 `icsp`；`icsp-csharp` 和 `self-adapt-icsp` 需要显式注册 C# 后端。" }
] ]
}, },
"POST /is_flyShotTrajValid/": { "POST /is_flyShotTrajValid/": {
@@ -256,7 +257,7 @@
"description": "检查指定飞拍轨迹是否可生成有效执行结果，并返回轨迹时长；底层未实现时会返回 501。", "description": "检查指定飞拍轨迹是否可生成有效执行结果，并返回轨迹时长；底层未实现时会返回 501。",
"requestBodyFields": [ "requestBodyFields": [
{ "name": "name", "type": "string", "required": true, "description": "要校验的飞拍轨迹名称。" }, { "name": "name", "type": "string", "required": true, "description": "要校验的飞拍轨迹名称。" },
{ "name": "method", "type": "string", "required": false, "description": "轨迹规划方法，默认 `icsp`。" }, { "name": "method", "type": "string", "required": false, "description": "轨迹规划方法，默认 `icsp`；`icsp-csharp` 和 `self-adapt-icsp` 需要显式注册 C# 后端。" },
{ "name": "save_traj", "type": "boolean", "required": false, "description": "是否同时保存轨迹信息，默认 `true`。" } { "name": "save_traj", "type": "boolean", "required": false, "description": "是否同时保存轨迹信息，默认 `true`。" }
] ]
}, },

BIN
src/Libs/flyshot_planning.dll Normal file
View File

Binary file not shown.

BIN
src/Libs/libflyshot_planning.so Normal file
View File

Binary file not shown.

64
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -16,70 +16,6 @@ namespace Flyshot.Core.Tests;
/// </summary> /// </summary>
public sealed class FanucControllerRuntimeDenseTests public sealed class FanucControllerRuntimeDenseTests
{ {
/// <summary>
/// 验证飞拍输出选择贯通到 J519 命令，DO/RO 模式都输出两周期脉冲并在同一路清零。
/// </summary>
/// <param name="useDo">true 选择 DO，false 选择 RO。</param>
/// <param name="expectedIoType">预期 J519 写入类型，DO 为 2，RO 为 9。</param>
/// <param name="usePreparedExecution">是否使用上游已完成触发绑定的最终发送队列。</param>
[Theory]
[InlineData(true, 2, false)]
[InlineData(false, 9, false)]
[InlineData(true, 2, true)]
[InlineData(false, 9, true)]
public void ExecuteTrajectory_FlyshotOutputSelection_PreservesPulseAndClear(bool useDo, int expectedIoType, bool usePreparedExecution)
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
runtime.ResetRobot(TestRobotFactory.CreateRobotProfile(), "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
var trigger = new TrajectoryDoEvent(waypointIndex: 1, triggerTime: 0.008,
offsetCycles: 0, holdCycles: 2, addressGroup: new IoAddressGroup([1]), useDo: useDo);
var samples = Enumerable.Range(0, 4)
.Select(index => new FlyshotPreparedSample(index, index * 0.008, index * 0.008,
1.0, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])).ToArray();
var preparedExecution = usePreparedExecution
? new FlyshotPreparedExecution(samples,
[new FlyshotPreparedTriggerBinding(trigger, samples[1], 1, true)],
Array.Empty<IReadOnlyList<double>>(), Array.Empty<IReadOnlyList<double>>(),
requestSpeedRatio: 1.0, finalSpeedRatio: 1.0, finalDurationSeconds: 0.024, stretchIterationCount: 0)
: null;
// 静止轨迹隔离运动算法影响，仅验证触发、保持和清零对应的协议字段。
var result = new TrajectoryResult(
programName: "output-selection",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: [trigger],
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2,
denseJointTrajectory:
[
new double[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new double[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
],
preparedFlyshotExecution: preparedExecution);
runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.True(commands.Count >= 4);
Assert.All(commands, command => Assert.Equal((byte)expectedIoType, command.WriteIoType));
Assert.All(commands, command => Assert.Equal((ushort)1, command.WriteIoIndex));
Assert.Equal(new ushort[] { 0, 1, 1, 1 }, commands.Take(4).Select(command => command.WriteIoMask));
Assert.Equal(new ushort[] { 0, 1, 1, 0 }, commands.Take(4).Select(command => command.WriteIoValue));
}
private const double SmoothPtpVelocityShapeCoefficient = 2.1875; private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293; private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
private const double SmoothPtpJerkShapeCoefficient = 52.5; private const double SmoothPtpJerkShapeCoefficient = 52.5;

25
tests/Flyshot.Core.Tests/FanucControllerRuntimeLoggingRegistrationTests.cs
View File

@@ -1,6 +1,5 @@
using Flyshot.Runtime.Common; using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc; using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.DependencyInjection; using Microsoft.Extensions.DependencyInjection;
using System.Reflection; using System.Reflection;
@@ -15,7 +14,7 @@ public sealed class FanucControllerRuntimeLoggingRegistrationTests
/// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。 /// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。
/// </summary> /// </summary>
[Fact] [Fact]
public void ServiceProvider_Resolves_FanucControllerRuntime_WithAllClientLoggers() public void ServiceProvider_Resolves_FanucControllerRuntime_WithLogger()
{ {
var services = new ServiceCollection(); var services = new ServiceCollection();
services.AddLogging(); services.AddLogging();
@@ -29,27 +28,5 @@ public sealed class FanucControllerRuntimeLoggingRegistrationTests
Assert.NotNull(loggerField); Assert.NotNull(loggerField);
Assert.NotNull(loggerField!.GetValue(concreteRuntime)); Assert.NotNull(loggerField!.GetValue(concreteRuntime));
AssertClientLoggerIsInjected<FanucCommandClient>(concreteRuntime, "_commandClient");
AssertClientLoggerIsInjected<FanucStateClient>(concreteRuntime, "_stateClient");
AssertClientLoggerIsInjected<FanucJ519Client>(concreteRuntime, "_j519Client");
}
/// <summary>
/// 断言运行时持有的底层协议客户端已注入对应类型的日志记录器。
/// </summary>
/// <typeparam name="TClient">待检查的协议客户端类型。</typeparam>
/// <param name="runtime">已经通过依赖注入解析出的 FANUC 运行时。</param>
/// <param name="clientFieldName">运行时内部保存该客户端实例的字段名。</param>
private static void AssertClientLoggerIsInjected<TClient>(FanucControllerRuntime runtime, string clientFieldName)
{
var clientField = typeof(FanucControllerRuntime).GetField(clientFieldName, BindingFlags.Instance | BindingFlags.NonPublic);
Assert.NotNull(clientField);
var client = Assert.IsType<TClient>(clientField!.GetValue(runtime));
var loggerField = typeof(TClient).GetField("_logger", BindingFlags.Instance | BindingFlags.NonPublic);
Assert.NotNull(loggerField);
Assert.NotNull(loggerField!.GetValue(client));
} }
} }

33
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
View File

@@ -279,34 +279,6 @@ public sealed class FanucJ519ClientTests : IDisposable
Assert.True(client.IsCommandQueueDrainedForTests()); Assert.True(client.IsCommandQueueDrainedForTests());
} }
/// <summary>
/// 验证机器人持续不可接收命令时，后续装载的命令队列会被完成，避免执行请求永久等待。
/// </summary>
[Fact]
public async Task LoadCommandQueue_WhenLatestStatusDoesNotAcceptCommands_CompletesQueueWait()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 900, status: 0x04);
await Task.Delay(100, _cts.Token);
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0])
]);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 901, status: 0x04);
await client.WaitForCommandQueueDrainedAsync(_cts.Token).WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
await Assert.ThrowsAsync<TimeoutException>(
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
Assert.True(client.IsCommandQueueDrainedForTests());
await client.StopMotionAsync(_cts.Token);
}
/// <summary> /// <summary>
/// 验证启用命令流结束开关后，LastData 结束帧发送完成会进入仅监控状态，不再继续回发保姿命令。 /// 验证启用命令流结束开关后，LastData 结束帧发送完成会进入仅监控状态，不再继续回发保姿命令。
/// </summary> /// </summary>
@@ -600,14 +572,13 @@ public sealed class FanucJ519ClientTests : IDisposable
private async Task SendStatusPacketAsync( private async Task SendStatusPacketAsync(
IPEndPoint clientEndpoint, IPEndPoint clientEndpoint,
uint sequence, uint sequence,
IReadOnlyList<double>? jointDegrees = null, IReadOnlyList<double>? jointDegrees = null)
byte status = 15)
{ {
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength]; var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0); BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1); BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence); BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
responsePacket[0x0c] = status; responsePacket[0x0c] = 15;
if (jointDegrees is not null) if (jointDegrees is not null)
{ {

8
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
View File

@@ -27,8 +27,16 @@
<ProjectReference Include="..\..\src\Flyshot.Core.Config\Flyshot.Core.Config.csproj" /> <ProjectReference Include="..\..\src\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" /> <ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" /> <ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\..\src\Flyshot.CSharp.Planning\Flyshot.CSharp.Planning.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" /> <ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Native.Planning\Flyshot.Native.Planning.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" /> <ProjectReference Include="..\..\src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
</ItemGroup> </ItemGroup>
<ItemGroup>
<!-- Rust ICSP 原生规划库：测试运行时需要与托管程序集同目录加载。 -->
<Content Include="..\..\src\Libs\flyshot_planning.dll" Link="flyshot_planning.dll" CopyToOutputDirectory="PreserveNewest" Condition="Exists('..\..\src\Libs\flyshot_planning.dll')" />
<Content Include="..\..\src\Libs\libflyshot_planning.so" Link="libflyshot_planning.so" CopyToOutputDirectory="PreserveNewest" Condition="Exists('..\..\src\Libs\libflyshot_planning.so')" />
</ItemGroup>
</Project> </Project>

3
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
View File

@@ -1,4 +1,5 @@
using Flyshot.Core.Config; using Flyshot.Core.Config;
using Flyshot.CSharp.Planning;
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning; using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export; using Flyshot.Core.Planning.Export;
@@ -55,7 +56,7 @@ public sealed class OfflinePlanTests
var request = new TrajectoryRequest( var request = new TrajectoryRequest(
robot: scaledProfile, robot: scaledProfile,
program: program, program: program,
method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.Icsp); method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.IcspCSharp);
PlannedTrajectory trajectory; PlannedTrajectory trajectory;
if (useSelfAdapt) if (useSelfAdapt)

5
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -1,4 +1,5 @@
using Flyshot.Core.Config; using Flyshot.Core.Config;
using Flyshot.CSharp.Planning;
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning; using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling; using Flyshot.Core.Planning.Sampling;
@@ -27,7 +28,7 @@ public sealed class PlanningCompatibilityTests
new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 }, new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 },
new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 } new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 }
}), }),
method: PlanningMethod.Icsp); method: PlanningMethod.IcspCSharp);
var trajectory = new ICspPlanner().Plan(request); var trajectory = new ICspPlanner().Plan(request);
@@ -51,7 +52,7 @@ public sealed class PlanningCompatibilityTests
new[] { 20.0 }, new[] { 20.0 },
new[] { 30.0 } new[] { 30.0 }
}), }),
method: PlanningMethod.Icsp); method: PlanningMethod.IcspCSharp);
var planner = new ICspPlanner(maxIterations: 0); var planner = new ICspPlanner(maxIterations: 0);

241
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -1,4 +1,5 @@
using Flyshot.ControllerClientCompat; using Flyshot.ControllerClientCompat;
using Flyshot.CSharp.Planning;
using Flyshot.Core.Config; using Flyshot.Core.Config;
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning; using Flyshot.Core.Planning;
@@ -16,6 +17,19 @@ namespace Flyshot.Core.Tests;
/// </summary> /// </summary>
public sealed class RuntimeOrchestrationTests public sealed class RuntimeOrchestrationTests
{ {
/// <summary>
/// 创建显式注册 C# ICSP 后端的编排器，用于验证可选算法 DLL 的行为边界。
/// </summary>
/// <returns>包含 C# 普通 ICSP 与 self-adapt-icsp 后端的轨迹编排器。</returns>
private static ControllerClientTrajectoryOrchestrator CreateOrchestratorWithCSharpBackends()
{
return new ControllerClientTrajectoryOrchestrator(
[
new CSharpIcspPlanningBackend(),
new SelfAdaptIcspPlanningBackend()
]);
}
/// <summary> /// <summary>
/// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。 /// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
/// </summary> /// </summary>
@@ -106,6 +120,155 @@ public sealed class RuntimeOrchestrationTests
Assert.Single(bundle.Result.TriggerTimeline); Assert.Single(bundle.Result.TriggerTimeline);
} }
/// <summary>
/// 验证普通轨迹默认 icsp 会真实调用 Rust DLL，并在结果中保留正式 icsp 方法标记。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_DefaultIcspUsesRustPlanner()
{
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);
}
/// <summary>
/// 验证旧的 icsp_rust 字串不再作为正式入口，并提示调用方改用 icsp。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsLegacyIcspRustName()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var exception = Assert.Throws<ArgumentException>(() => 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.Contains("icsp", exception.Message);
Assert.Contains("icsp_rust", exception.Message);
}
/// <summary>
/// 验证未注册 C# 后端时，icsp-csharp 会明确报错而不是静默回退。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsCSharpIcspWhenBackendIsMissing()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var exception = Assert.Throws<NotSupportedException>(() => 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-csharp")));
Assert.Contains("icsp-csharp", exception.Message);
}
/// <summary>
/// 验证显式注册 C# 后端后，icsp-csharp 使用拆分后的托管规划器。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_UsesRegisteredCSharpIcspPlanner()
{
var orchestrator = CreateOrchestratorWithCSharpBackends();
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-csharp"));
Assert.Equal(PlanningMethod.IcspCSharp, bundle.PlannedTrajectory.Method);
Assert.Equal(PlanningMethod.IcspCSharp, bundle.Result.Method);
}
/// <summary>
/// 验证未注册 C# 后端时，self-adapt-icsp 会明确报错而不是把行为藏在主服务里。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_RejectsSelfAdaptWhenBackendIsMissing()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var exception = Assert.Throws<NotSupportedException>(() => orchestrator.PlanUploadedFlyshot(
robot,
uploaded,
new FlyshotExecutionOptions(method: "self-adapt-icsp", useCache: false)));
Assert.Contains("self-adapt-icsp", exception.Message);
}
/// <summary>
/// 验证显式注册 C# 后端后，self-adapt-icsp 仍使用 C# 自适应规划器。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRegisteredManagedSelfAdaptPlanner()
{
var orchestrator = CreateOrchestratorWithCSharpBackends();
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);
}
/// <summary> /// <summary>
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。 /// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
/// </summary> /// </summary>
@@ -241,14 +404,13 @@ public sealed class RuntimeOrchestrationTests
var doEvent = Assert.Single(bundle.Result.TriggerTimeline); var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
Assert.Equal(4, doEvent.HoldCycles); Assert.Equal(4, doEvent.HoldCycles);
Assert.True(doEvent.UseDo);
} }
/// <summary> /// <summary>
/// 验证 use_do=false 时仍生成飞拍触发事件，保持周期不变。 /// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
/// </summary> /// </summary>
[Fact] [Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_PreservesTriggerTimeline_WhenUseDoIsFalse() public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
{ {
var orchestrator = new ControllerClientTrajectoryOrchestrator(); var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile(); var robot = TestRobotFactory.CreateRobotProfile();
@@ -265,9 +427,7 @@ public sealed class RuntimeOrchestrationTests
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings); var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
Assert.Single(bundle.Result.ShotEvents); Assert.Single(bundle.Result.ShotEvents);
var trigger = Assert.Single(bundle.Result.TriggerTimeline); Assert.Empty(bundle.Result.TriggerTimeline);
Assert.Equal(4, trigger.HoldCycles);
Assert.False(trigger.UseDo);
} }
/// <summary> /// <summary>
@@ -791,66 +951,6 @@ public sealed class RuntimeOrchestrationTests
} }
} }
/// <summary>
/// 验证直接关节移动和飞拍预移动均按规划倍率与当前运行倍率的乘积生成点对点轨迹。
/// </summary>
/// <param name="moveToStart">是否通过飞拍预移动入口执行点对点运动。</param>
/// <param name="planningSpeedScale">配置中的规划速度倍率。</param>
/// <param name="speedRatio">机器人初始化后设置的当前运行速度倍率。</param>
[Theory]
[InlineData(false, 0.74227, 1.0)]
[InlineData(false, 0.74227, 0.5)]
[InlineData(false, 1.0, 0.7)]
[InlineData(true, 0.74227, 0.5)]
public void ControllerClientCompatService_MoveJoint_AppliesCombinedPlanningSpeedScale(
bool moveToStart, double planningSpeedScale, double speedRatio)
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot, planningSpeedScale: planningSpeedScale, speedRatio: 1.0);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
double[] startJoints = [0.4, 0.0, 0.0, 0.0, 0.0, 0.0];
double[] targetJoints = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
var runtime = new DelayedCompletionControllerRuntime(startJoints, TimeSpan.Zero);
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.SetSpeedRatio(speedRatio);
// 两个公开入口必须共享相同的点对点限速，且采用当前倍率而非配置默认倍率。
if (moveToStart)
{
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
service.ExecuteTrajectoryByName("demo-flyshot", new FlyshotExecutionOptions(moveToStart: true, useCache: false));
}
else
{
service.MoveJoint(targetJoints);
}
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
var expected = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, planningSpeedScale * speedRatio);
var actual = runtime.ExecuteCalls[0].Result;
Assert.Equal(expected.Duration, actual.Duration);
Assert.Equal(expected.DenseJointTrajectory!.Count, actual.DenseJointTrajectory!.Count);
Assert.Equal(targetJoints, runtime.ExecuteCalls[0].FinalJointPositions);
Assert.Equal(speedRatio, runtime.GetSnapshot().SpeedRatio);
Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary> /// <summary>
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。 /// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
/// </summary> /// </summary>
@@ -2289,10 +2389,6 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
/// </summary> /// </summary>
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
{ {
/// <summary>
/// 当前测试运行速度倍率，用于验证初始化与运行中调速对预移动规划的影响。
/// </summary>
private double _speedRatio = 1.0;
private readonly object _lock = new(); private readonly object _lock = new();
private readonly TimeSpan _firstMotionCompletionDelay; private readonly TimeSpan _firstMotionCompletionDelay;
private double[] _jointPositions; private double[] _jointPositions;
@@ -2365,12 +2461,11 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
} }
/// <inheritdoc /> /// <inheritdoc />
public double GetSpeedRatio() => _speedRatio; public double GetSpeedRatio() => 1.0;
/// <inheritdoc /> /// <inheritdoc />
public void SetSpeedRatio(double ratio) public void SetSpeedRatio(double ratio)
{ {
_speedRatio = ratio;
} }
/// <inheritdoc /> /// <inheritdoc />
@@ -2411,7 +2506,7 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
connectionState: "Connected", connectionState: "Connected",
isEnabled: _isEnabled, isEnabled: _isEnabled,
isInMotion: _isInMotion, isInMotion: _isInMotion,
speedRatio: _speedRatio, speedRatio: 1.0,
jointPositions: _jointPositions.ToArray(), jointPositions: _jointPositions.ToArray(),
cartesianPose: Array.Empty<double>(), cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>()); activeAlarms: Array.Empty<RuntimeAlarm>());

136
tests/Flyshot.Core.Tests/RustIcspPlannerInteropTests.cs Normal file
View File

@@ -0,0 +1,136 @@
using Flyshot.Core.Domain;
using Flyshot.CSharp.Planning;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 Rust ICSP 托管互操作边界，包括原生库加载失败时的诊断信息。
/// </summary>
public sealed class RustIcspPlannerInteropTests
{
/// <summary>
/// 验证显式指定不存在的原生库时，规划器会抛出包含库路径的明确异常。
/// </summary>
[Fact]
public void RustIcspPlanner_ThrowsClearError_WhenNativeLibraryIsMissing()
{
var planner = new RustIcspPlanner(
options: NativeIcspPlannerOptions.Default,
libraryResolver: new NativePlanningLibraryResolver(_ => "missing-library"));
var exception = Assert.Throws<NativePlanningException>(() => planner.Plan(CreateSimpleRequest()));
Assert.Contains("missing-library", exception.Message);
}
/// <summary>
/// 验证实际原生规划结果会被复制为现有 <see cref="Flyshot.Core.Planning.PlannedTrajectory"/>。
/// </summary>
[Fact]
public void RustIcspPlanner_ReturnsManagedPlannedTrajectory_FromNativeResult()
{
var planner = new RustIcspPlanner(
NativeIcspPlannerOptions.Default with { EnforceFinalScale = false });
var result = planner.Plan(CreateSimpleRequest());
Assert.Equal(PlanningMethod.Icsp, 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);
}
/// <summary>
/// 验证互操作测试夹具在两套实现中都由相同的最终 scale 规则拒绝。
/// </summary>
[Fact]
public void ManagedIcspPlanner_RejectsInteropFixtureWithExpectedFinalScale()
{
var planner = new ICspPlanner();
var exception = Assert.Throws<InvalidOperationException>(
() => planner.Plan(CreateSimpleRequest()));
Assert.Contains("global_scale=1.058684", exception.Message);
}
/// <summary>
/// 验证进程已加载默认 DLL 后，显式指定其他缺失库仍会暴露部署错误。
/// </summary>
[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<NativePlanningException>(
() => missingPlanner.Plan(CreateSimpleRequest()));
Assert.Contains("different-missing-library", exception.Message);
}
/// <summary>
/// 验证 Rust 未收敛错误码和线程局部错误文本会转换为可诊断的托管异常。
/// </summary>
[Fact]
public void RustIcspPlanner_ConvertsNativeNotConvergedError()
{
var planner = new RustIcspPlanner(
NativeIcspPlannerOptions.Default with { MaxIterations = 0 });
var exception = Assert.Throws<NativePlanningException>(
() => planner.Plan(CreateSimpleRequest()));
Assert.Equal(3, exception.ErrorCodeValue);
Assert.Contains("global_scale", exception.Message);
}
/// <summary>
/// 创建用于互操作边界测试的最小四路点单自由度规划请求。
/// </summary>
/// <returns>满足 ICSP 最小路点数量要求的轨迹请求。</returns>
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);
}
}

161
tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs Normal file
View File

@@ -0,0 +1,161 @@
using Flyshot.Core.Domain;
using Flyshot.CSharp.Planning;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 Rust ICSP 与现有 C# 基准实现保持逐项数值一致。
/// </summary>
public sealed class RustIcspPlannerParityTests
{
/// <summary>
/// 验证基础六自由度兼容样本的时间轴、段时长、段缩放和迭代信息完全对齐。
/// </summary>
[Fact]
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture()
{
TrajectoryRequest request = CreateCompatibilityRequest();
PlannedTrajectory managed = new ICspPlanner().Plan(request);
PlannedTrajectory native = new RustIcspPlanner().Plan(request);
Assert.Equal(PlanningMethod.IcspCSharp, managed.Method);
Assert.Equal(PlanningMethod.Icsp, native.Method);
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
}
/// <summary>
/// 验证关闭最终 scale 拒绝后，多自由度非均匀路点仍与托管算法逐项对齐。
/// </summary>
[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.IcspCSharp, managed.Method);
Assert.Equal(PlanningMethod.Icsp, native.Method);
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
}
/// <summary>
/// 创建与现有 PlanningCompatibilityTests 基础用例一致的六自由度请求。
/// </summary>
/// <returns>可由默认 ICSP 配置收敛的稳定规划请求。</returns>
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]
]);
}
/// <summary>
/// 创建用于覆盖多自由度、非均匀段距离和不同约束的规划请求。
/// </summary>
/// <returns>关闭最终 scale 拒绝后可用于数值对拍的稳定请求。</returns>
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]
]);
}
/// <summary>
/// 根据逐关节限制和路点矩阵创建完整领域请求。
/// </summary>
/// <param name="velocityLimits">逐关节速度上限。</param>
/// <param name="accelerationLimits">逐关节加速度上限。</param>
/// <param name="jerkLimits">逐关节 jerk 上限。</param>
/// <param name="waypointRows">按路点排列的关节位置矩阵。</param>
/// <returns>可同时传给 C# 与 Rust 规划器的请求。</returns>
private static TrajectoryRequest CreateRequest(
IReadOnlyList<double> velocityLimits,
IReadOnlyList<double> accelerationLimits,
IReadOnlyList<double> jerkLimits,
IReadOnlyList<double[]> 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.IcspCSharp);
}
/// <summary>
/// 逐字段比较两套规划实现的数值结果。
/// </summary>
/// <param name="managed">现有 C# ICSP 基准结果。</param>
/// <param name="native">Rust ICSP 原生结果。</param>
/// <param name="tolerance">所有浮点数组和 threshold 使用的绝对误差。</param>
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);
}
/// <summary>
/// 按相同索引比较两个浮点序列的绝对误差。
/// </summary>
/// <param name="expected">托管基准序列。</param>
/// <param name="actual">Rust 原生序列。</param>
/// <param name="tolerance">允许的最大绝对误差。</param>
private static void AssertSequenceMatches(
IReadOnlyList<double> expected,
IReadOnlyList<double> actual,
double tolerance)
{
for (int index = 0; index < expected.Count; index++)
{
Assert.InRange(Math.Abs(expected[index] - actual[index]), 0.0, tolerance);
}
}
}