feat: 初始化飞拍替换方案仓库骨架

* 建立 .NET 8 解决方案及分层项目结构
* 添加 Flyshot.Core.Domain 领域模型(机器人、轨迹、运动学)
* 添加 Flyshot.Core.Planning 规划层(ICSP、CubicSpline、采样器)
* 添加 Flyshot.Core.Triggering 触发时序与 IO 时间轴
* 添加 Flyshot.Core.Config 配置兼容与 .robot 解析
* 添加 Flyshot.Server.Host 最小宿主及 /healthz 端点
* 补充单元测试与集成测试项目
* 添加 CLAUDE.md、AGENTS.md、README.md 项目规范
This commit is contained in:
2026-04-23 17:35:37 +08:00
commit 4eeaa3fef3
47 changed files with 5140 additions and 0 deletions

398
.gitignore vendored Normal file
View File

@@ -0,0 +1,398 @@
## Ignore Visual Studio temporary files, build results, and
## files generated by popular Visual Studio add-ons.
##
## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
# User-specific files
*.rsuser
*.suo
*.user
*.userosscache
*.sln.docstates
# User-specific files (MonoDevelop/Xamarin Studio)
*.userprefs
# Mono auto generated files
mono_crash.*
# Build results
[Dd]ebug/
[Dd]ebugPublic/
[Rr]elease/
[Rr]eleases/
x64/
x86/
[Ww][Ii][Nn]32/
[Aa][Rr][Mm]/
[Aa][Rr][Mm]64/
bld/
[Bb]in/
[Oo]bj/
[Ll]og/
[Ll]ogs/
# Visual Studio 2015/2017 cache/options directory
.vs/
# Uncomment if you have tasks that create the project's static files in wwwroot
#wwwroot/
# Visual Studio 2017 auto generated files
Generated\ Files/
# MSTest test Results
[Tt]est[Rr]esult*/
[Bb]uild[Ll]og.*
# NUnit
*.VisualState.xml
TestResult.xml
nunit-*.xml
# Build Results of an ATL Project
[Dd]ebugPS/
[Rr]eleasePS/
dlldata.c
# Benchmark Results
BenchmarkDotNet.Artifacts/
# .NET Core
project.lock.json
project.fragment.lock.json
artifacts/
# ASP.NET Scaffolding
ScaffoldingReadMe.txt
# StyleCop
StyleCopReport.xml
# Files built by Visual Studio
*_i.c
*_p.c
*_h.h
*.ilk
*.meta
*.obj
*.iobj
*.pch
*.pdb
*.ipdb
*.pgc
*.pgd
*.rsp
*.sbr
*.tlb
*.tli
*.tlh
*.tmp
*.tmp_proj
*_wpftmp.csproj
*.log
*.tlog
*.vspscc
*.vssscc
.builds
*.pidb
*.svclog
*.scc
# Chutzpah Test files
_Chutzpah*
# Visual C++ cache files
ipch/
*.aps
*.ncb
*.opendb
*.opensdf
*.sdf
*.cachefile
*.VC.db
*.VC.VC.opendb
# Visual Studio profiler
*.psess
*.vsp
*.vspx
*.sap
# Visual Studio Trace Files
*.e2e
# TFS 2012 Local Workspace
$tf/
# Guidance Automation Toolkit
*.gpState
# ReSharper is a .NET coding add-in
_ReSharper*/
*.[Rr]e[Ss]harper
*.DotSettings.user
# TeamCity is a build add-in
_TeamCity*
# DotCover is a Code Coverage Tool
*.dotCover
# AxoCover is a Code Coverage Tool
.axoCover/*
!.axoCover/settings.json
# Coverlet is a free, cross platform Code Coverage Tool
coverage*.json
coverage*.xml
coverage*.info
# Visual Studio code coverage results
*.coverage
*.coveragexml
# NCrunch
_NCrunch_*
.*crunch*.local.xml
nCrunchTemp_*
# MightyMoose
*.mm.*
AutoTest.Net/
# Web workbench (sass)
.sass-cache/
# Installshield output folder
[Ee]xpress/
# DocProject is a documentation generator add-in
DocProject/buildhelp/
DocProject/Help/*.HxT
DocProject/Help/*.HxC
DocProject/Help/*.hhc
DocProject/Help/*.hhk
DocProject/Help/*.hhp
DocProject/Help/Html2
DocProject/Help/html
# Click-Once directory
publish/
# Publish Web Output
*.[Pp]ublish.xml
*.azurePubxml
# Note: Comment the next line if you want to checkin your web deploy settings,
# but database connection strings (with potential passwords) will be unencrypted
*.pubxml
*.publishproj
# Microsoft Azure Web App publish settings. Comment the next line if you want to
# checkin your Azure Web App publish settings, but sensitive information contained
# in these scripts will be unencrypted
PublishScripts/
# NuGet Packages
*.nupkg
# NuGet Symbol Packages
*.snupkg
# The packages folder can be ignored because of Package Restore
**/[Pp]ackages/*
# except build/, which is used as an MSBuild target.
!**/[Pp]ackages/build/
# Uncomment if necessary however generally it will be regenerated when needed
#!**/[Pp]ackages/repositories.config
# NuGet v3's project.json files produces more ignorable files
*.nuget.props
*.nuget.targets
# Microsoft Azure Build Output
csx/
*.build.csdef
# Microsoft Azure Emulator
ecf/
rcf/
# Windows Store app package directories and files
AppPackages/
BundleArtifacts/
Package.StoreAssociation.xml
_pkginfo.txt
*.appx
*.appxbundle
*.appxupload
# Visual Studio cache files
# files ending in .cache can be ignored
*.[Cc]ache
# but keep track of directories ending in .cache
!?*.[Cc]ache/
# Others
ClientBin/
~$*
*~
*.dbmdl
*.dbproj.schemaview
*.jfm
*.pfx
*.publishsettings
orleans.codegen.cs
# Including strong name files can present a security risk
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
#*.snk
# Since there are multiple workflows, uncomment next line to ignore bower_components
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
#bower_components/
# RIA/Silverlight projects
Generated_Code/
# Backup & report files from converting an old project file
# to a newer Visual Studio version. Backup files are not needed,
# because we have git ;-)
_UpgradeReport_Files/
Backup*/
UpgradeLog*.XML
UpgradeLog*.htm
ServiceFabricBackup/
*.rptproj.bak
# SQL Server files
*.mdf
*.ldf
*.ndf
# Business Intelligence projects
*.rdl.data
*.bim.layout
*.bim_*.settings
*.rptproj.rsuser
*- [Bb]ackup.rdl
*- [Bb]ackup ([0-9]).rdl
*- [Bb]ackup ([0-9][0-9]).rdl
# Microsoft Fakes
FakesAssemblies/
# GhostDoc plugin setting file
*.GhostDoc.xml
# Node.js Tools for Visual Studio
.ntvs_analysis.dat
node_modules/
# Visual Studio 6 build log
*.plg
# Visual Studio 6 workspace options file
*.opt
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
*.vbw
# Visual Studio 6 auto-generated project file (contains which files were open etc.)
*.vbp
# Visual Studio 6 workspace and project file (working project files containing files to include in project)
*.dsw
*.dsp
# Visual Studio 6 technical files
*.ncb
*.aps
# Visual Studio LightSwitch build output
**/*.HTMLClient/GeneratedArtifacts
**/*.DesktopClient/GeneratedArtifacts
**/*.DesktopClient/ModelManifest.xml
**/*.Server/GeneratedArtifacts
**/*.Server/ModelManifest.xml
_Pvt_Extensions
# Paket dependency manager
.paket/paket.exe
paket-files/
# FAKE - F# Make
.fake/
# CodeRush personal settings
.cr/personal
# Python Tools for Visual Studio (PTVS)
__pycache__/
*.pyc
# Cake - Uncomment if you are using it
# tools/**
# !tools/packages.config
# Tabs Studio
*.tss
# Telerik's JustMock configuration file
*.jmconfig
# BizTalk build output
*.btp.cs
*.btm.cs
*.odx.cs
*.xsd.cs
# OpenCover UI analysis results
OpenCover/
# Azure Stream Analytics local run output
ASALocalRun/
# MSBuild Binary and Structured Log
*.binlog
# NVidia Nsight GPU debugger configuration file
*.nvuser
# MFractors (Xamarin productivity tool) working folder
.mfractor/
# Local History for Visual Studio
.localhistory/
# Visual Studio History (VSHistory) files
.vshistory/
# BeatPulse healthcheck temp database
healthchecksdb
# Backup folder for Package Reference Convert tool in Visual Studio 2017
MigrationBackup/
# Ionide (cross platform F# VS Code tools) working folder
.ionide/
# Fody - auto-generated XML schema
FodyWeavers.xsd
# VS Code files for those working on multiple tools
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
*.code-workspace
# Local History for Visual Studio Code
.history/
# Windows Installer files from build outputs
*.cab
*.msi
*.msix
*.msm
*.msp
# JetBrains Rider
*.sln.iml

150
AGENTS.md Normal file
View File

@@ -0,0 +1,150 @@
# Flyshot Replacement 仓库规范
## 1. 仓库目标
本仓库用于重写并替代现有 RVBUST/FANUC 飞拍服务端链路。
第一版目标固定为:
- 使用 `C# + .NET 8`
- 提供跨平台独立服务端
- 兼容现有 `50001/TCP+JSON` 上层接入语义
- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控
- Windows / Linux 都能运行完整服务端
- 只支持当前现场这套组合
明确不做:
- GUI 桌面程序
- 多机器人同时控制
- 面向多控制柜的通用平台化框架
## 2. 代码与资料边界
本仓库是新的独立实现仓库,不复用旧的 RVBUST 二进制作为运行时依赖。
允许依赖的内容:
- 旧仓库中的逆向分析文档
- 旧仓库中的协议样本、轨迹样本和配置样本
- 旧仓库中的 `ControllerClient.h` 公开接口
不允许把旧服务端实现直接包装成“新系统”。
## 3. 当前目录结构
```text
flyshot-replacement/
├─ src/
│ ├─ Flyshot.Server.Host/
│ ├─ Flyshot.Core.Domain/
│ └─ Flyshot.Runtime.Common/
├─ tests/
│ ├─ Flyshot.Server.IntegrationTests/
│ └─ Flyshot.Core.Tests/
├─ FlyshotReplacement.sln
├─ Directory.Build.props
├─ README.md
└─ AGENTS.md
```
后续新增模块时,优先保持以下边界:
- `Flyshot.Core.Domain`
- 纯领域对象
- 不依赖网络、文件系统、HTTP、UI、操作系统
- `Flyshot.Core.Config`
- 配置兼容
- `.robot` 解析
- 路径兼容
- `Flyshot.Core.Planning`
- `icsp`
- `self-adapt-icsp`
- `doubles`
- `Flyshot.Core.Triggering`
- `TrajectoryDO` 等价时间轴
- `shot_flags / offset_values / addr` 解析
- `Flyshot.LegacyGateway`
- `50001/TCP+JSON` 兼容接入
- `Flyshot.Runtime.Fanuc`
- `10010 / 10012 / 60015`
- `Flyshot.Web.Status`
- 状态查看
- 诊断页
- 不直接处理底层协议细节
## 4. 开发规范
### 4.1 通用要求
- 正式文档默认使用中文。
- 不要把临时验证脚本直接塞进正式运行时代码。
- 兼容性优先级高于“重新发明接口”。
- 第一版默认围绕当前现场组合实现,不提前做泛化设计。
### 4.2 实现约束
- 旧协议兼容以“语义兼容”为主,不追求二进制逐字节一致。
- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。
- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。
- 网页只做状态监控,不把复杂控制流程放进前端。
- 多机型切换通过不同 `.robot` 文件加载,不在第一版做插件化机型框架。
### 4.3 测试要求
- 默认采用 TDD。
- 至少先写失败测试,再写最小实现。
- 每完成一个阶段,要补最小可运行验证,而不是只看代码编译是否“像是对的”。
### 4.4 注释要求
- 所有生成代码都必须带中文注释,不可缺漏。
- 所有类定义都必须在类头提供 XML 注释。
- 所有静态变量都必须提供 XML 注释。
- 关键代码块必须补充单行注释,说明该段逻辑为什么存在、在做什么,不允许只写空泛注释。
## 5. 构建与验证命令
在当前环境中,推荐使用下面两条命令:
```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'
/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal'
```
说明:
- 第一条命令当前已经验证通过,可用于检查最小宿主和 `/healthz`
- 第二条命令当前已经验证通过,可用于检查解决方案骨架是否完整。
后续新增模块时,继续补充:
- `Flyshot.Core.Tests`
- 协议回放测试
- golden sample 对拍测试
- 端到端集成测试
## 6. 修改前优先查看的资料
本仓库上层是独立实现,但上下文仍然依赖父目录中的逆向资料。开始重要改动前,优先阅读:
- `../analysis/ControllerServer_analysis.md`
- `../analysis/ICSP_algorithm_reverse_analysis.md`
- `../analysis/CommonMsg_protocol_analysis.md`
- `../analysis/J519_stream_motion_analysis.md`
- `../analysis/FANUC_realtime_comm_analysis.md`
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
## 7. 任务推进方式
- `README.md` 中的 Todo 需要随着阶段推进同步更新。
- 如果实现范围发生收敛或扩展,先更新设计或计划,再继续改代码。
- 如果发现计划漏了关键对象或模块,直接补到文档里,不要把缺口留到后面。
## 8. 当前已验证状态
- 独立仓库已初始化。
- `dotnet 8` 解决方案骨架已建立。
- `Flyshot.Server.Host` 已提供最小 `/healthz`
- 最小集成测试已通过。
- 解决方案构建已通过。

150
CLAUDE.md Normal file
View File

@@ -0,0 +1,150 @@
# Flyshot Replacement 仓库规范
## 1. 仓库目标
本仓库用于重写并替代现有 RVBUST/FANUC 飞拍服务端链路。
第一版目标固定为:
- 使用 `C# + .NET 8`
- 提供跨平台独立服务端
- 兼容现有 `50001/TCP+JSON` 上层接入语义
- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控
- Windows / Linux 都能运行完整服务端
- 只支持当前现场这套组合
明确不做:
- GUI 桌面程序
- 多机器人同时控制
- 面向多控制柜的通用平台化框架
## 2. 代码与资料边界
本仓库是新的独立实现仓库,不复用旧的 RVBUST 二进制作为运行时依赖。
允许依赖的内容:
- 旧仓库中的逆向分析文档
- 旧仓库中的协议样本、轨迹样本和配置样本
- 旧仓库中的 `ControllerClient.h` 公开接口
不允许把旧服务端实现直接包装成“新系统”。
## 3. 当前目录结构
```text
flyshot-replacement/
├─ src/
│ ├─ Flyshot.Server.Host/
│ ├─ Flyshot.Core.Domain/
│ └─ Flyshot.Runtime.Common/
├─ tests/
│ ├─ Flyshot.Server.IntegrationTests/
│ └─ Flyshot.Core.Tests/
├─ FlyshotReplacement.sln
├─ Directory.Build.props
├─ README.md
└─ AGENTS.md
```
后续新增模块时,优先保持以下边界:
- `Flyshot.Core.Domain`
- 纯领域对象
- 不依赖网络、文件系统、HTTP、UI、操作系统
- `Flyshot.Core.Config`
- 配置兼容
- `.robot` 解析
- 路径兼容
- `Flyshot.Core.Planning`
- `icsp`
- `self-adapt-icsp`
- `doubles`
- `Flyshot.Core.Triggering`
- `TrajectoryDO` 等价时间轴
- `shot_flags / offset_values / addr` 解析
- `Flyshot.LegacyGateway`
- `50001/TCP+JSON` 兼容接入
- `Flyshot.Runtime.Fanuc`
- `10010 / 10012 / 60015`
- `Flyshot.Web.Status`
- 状态查看
- 诊断页
- 不直接处理底层协议细节
## 4. 开发规范
### 4.1 通用要求
- 正式文档默认使用中文。
- 不要把临时验证脚本直接塞进正式运行时代码。
- 兼容性优先级高于“重新发明接口”。
- 第一版默认围绕当前现场组合实现,不提前做泛化设计。
### 4.2 实现约束
- 旧协议兼容以“语义兼容”为主,不追求二进制逐字节一致。
- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。
- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。
- 网页只做状态监控,不把复杂控制流程放进前端。
- 多机型切换通过不同 `.robot` 文件加载,不在第一版做插件化机型框架。
### 4.3 测试要求
- 默认采用 TDD。
- 至少先写失败测试,再写最小实现。
- 每完成一个阶段,要补最小可运行验证,而不是只看代码编译是否“像是对的”。
### 4.4 注释要求
- 所有生成代码都必须带中文注释,不可缺漏。
- 所有类定义都必须在类头提供 XML 注释。
- 所有静态变量都必须提供 XML 注释。
- 关键代码块必须补充单行注释,说明该段逻辑为什么存在、在做什么,不允许只写空泛注释。
## 5. 构建与验证命令
在当前环境中,推荐使用下面两条命令:
```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'
/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal'
```
说明:
- 第一条命令当前已经验证通过,可用于检查最小宿主和 `/healthz`
- 第二条命令当前已经验证通过,可用于检查解决方案骨架是否完整。
后续新增模块时,继续补充:
- `Flyshot.Core.Tests`
- 协议回放测试
- golden sample 对拍测试
- 端到端集成测试
## 6. 修改前优先查看的资料
本仓库上层是独立实现,但上下文仍然依赖父目录中的逆向资料。开始重要改动前,优先阅读:
- `../analysis/ControllerServer_analysis.md`
- `../analysis/ICSP_algorithm_reverse_analysis.md`
- `../analysis/CommonMsg_protocol_analysis.md`
- `../analysis/J519_stream_motion_analysis.md`
- `../analysis/FANUC_realtime_comm_analysis.md`
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
## 7. 任务推进方式
- `README.md` 中的 Todo 需要随着阶段推进同步更新。
- 如果实现范围发生收敛或扩展,先更新设计或计划,再继续改代码。
- 如果发现计划漏了关键对象或模块,直接补到文档里,不要把缺口留到后面。
## 8. 当前已验证状态
- 独立仓库已初始化。
- `dotnet 8` 解决方案骨架已建立。
- `Flyshot.Server.Host` 已提供最小 `/healthz`
- 最小集成测试已通过。
- 解决方案构建已通过。

8
Directory.Build.props Normal file
View File

@@ -0,0 +1,8 @@
<Project>
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<Nullable>enable</Nullable>
<ImplicitUsings>enable</ImplicitUsings>
<LangVersion>12.0</LangVersion>
</PropertyGroup>
</Project>

146
FlyshotReplacement.sln Normal file
View File

@@ -0,0 +1,146 @@

Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.0.31903.59
MinimumVisualStudioVersion = 10.0.40219.1
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "src", "src", "{64EABE09-B1E0-4476-A213-32C93E46E7C3}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Server.Host", "src\Flyshot.Server.Host\Flyshot.Server.Host.csproj", "{8A744541-C680-41D4-96D7-2E8C2E59D038}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Domain", "src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj", "{85691BA7-BBD1-4A5B-A141-D354200299AF}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Config", "src\Flyshot.Core.Config\Flyshot.Core.Config.csproj", "{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Common", "src\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj", "{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}"
EndProject
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "tests", "tests", "{CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Server.IntegrationTests", "tests\Flyshot.Server.IntegrationTests\Flyshot.Server.IntegrationTests.csproj", "{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Tests", "tests\Flyshot.Core.Tests\Flyshot.Core.Tests.csproj", "{6CC8418D-2A13-4D70-8F94-585CD71F0B74}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Planning", "src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj", "{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}"
EndProject
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Triggering", "src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj", "{E4DDC34C-9AB6-4050-A927-3DF69804708A}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Any CPU = Debug|Any CPU
Debug|x64 = Debug|x64
Debug|x86 = Debug|x86
Release|Any CPU = Release|Any CPU
Release|x64 = Release|x64
Release|x86 = Release|x86
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|Any CPU.Build.0 = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x64.ActiveCfg = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x64.Build.0 = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x86.ActiveCfg = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Debug|x86.Build.0 = Debug|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|Any CPU.ActiveCfg = Release|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|Any CPU.Build.0 = Release|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x64.ActiveCfg = Release|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x64.Build.0 = Release|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x86.ActiveCfg = Release|Any CPU
{8A744541-C680-41D4-96D7-2E8C2E59D038}.Release|x86.Build.0 = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|Any CPU.Build.0 = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x64.ActiveCfg = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x64.Build.0 = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x86.ActiveCfg = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Debug|x86.Build.0 = Debug|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|Any CPU.ActiveCfg = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|Any CPU.Build.0 = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x64.ActiveCfg = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x64.Build.0 = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x86.ActiveCfg = Release|Any CPU
{85691BA7-BBD1-4A5B-A141-D354200299AF}.Release|x86.Build.0 = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|Any CPU.Build.0 = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x64.ActiveCfg = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x64.Build.0 = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x86.ActiveCfg = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Debug|x86.Build.0 = Debug|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|Any CPU.ActiveCfg = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|Any CPU.Build.0 = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x64.ActiveCfg = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x64.Build.0 = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x86.ActiveCfg = Release|Any CPU
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550}.Release|x86.Build.0 = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|Any CPU.Build.0 = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x64.ActiveCfg = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x64.Build.0 = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x86.ActiveCfg = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Debug|x86.Build.0 = Debug|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|Any CPU.ActiveCfg = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|Any CPU.Build.0 = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x64.ActiveCfg = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x64.Build.0 = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x86.ActiveCfg = Release|Any CPU
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C}.Release|x86.Build.0 = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|Any CPU.Build.0 = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x64.ActiveCfg = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x64.Build.0 = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x86.ActiveCfg = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Debug|x86.Build.0 = Debug|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|Any CPU.ActiveCfg = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|Any CPU.Build.0 = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x64.ActiveCfg = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x64.Build.0 = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x86.ActiveCfg = Release|Any CPU
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05}.Release|x86.Build.0 = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|Any CPU.Build.0 = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x64.ActiveCfg = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x64.Build.0 = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x86.ActiveCfg = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Debug|x86.Build.0 = Debug|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|Any CPU.ActiveCfg = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|Any CPU.Build.0 = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x64.ActiveCfg = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x64.Build.0 = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x86.ActiveCfg = Release|Any CPU
{6CC8418D-2A13-4D70-8F94-585CD71F0B74}.Release|x86.Build.0 = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|Any CPU.Build.0 = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x64.ActiveCfg = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x64.Build.0 = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x86.ActiveCfg = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Debug|x86.Build.0 = Debug|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|Any CPU.ActiveCfg = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|Any CPU.Build.0 = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x64.ActiveCfg = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x64.Build.0 = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x86.ActiveCfg = Release|Any CPU
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28}.Release|x86.Build.0 = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|Any CPU.Build.0 = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x64.ActiveCfg = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x64.Build.0 = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x86.ActiveCfg = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Debug|x86.Build.0 = Debug|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|Any CPU.ActiveCfg = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|Any CPU.Build.0 = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x64.ActiveCfg = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x64.Build.0 = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x86.ActiveCfg = Release|Any CPU
{E4DDC34C-9AB6-4050-A927-3DF69804708A}.Release|x86.Build.0 = Release|Any CPU
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
GlobalSection(NestedProjects) = preSolution
{8A744541-C680-41D4-96D7-2E8C2E59D038} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{85691BA7-BBD1-4A5B-A141-D354200299AF} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{D5DB99C2-D58A-49D0-8A7E-5D6D6273E550} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{B7E1F1B5-96FF-4C60-8AEE-A98D2278452C} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{7ADC4C1B-53B6-4456-8E6A-81DB38F13E05} = {CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE}
{6CC8418D-2A13-4D70-8F94-585CD71F0B74} = {CB517CF5-2EF6-43A8-B335-ABD3A6FCE3BE}
{154CA299-80D8-4BE2-B1C9-4BC133FA8B28} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
{E4DDC34C-9AB6-4050-A927-3DF69804708A} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
EndGlobalSection
EndGlobal

24
README.md Normal file
View File

@@ -0,0 +1,24 @@
# Flyshot Replacement
基于 `.NET 8` 的跨平台飞拍服务端重写项目。
当前目标:
- 兼容现有 `50001/TCP+JSON` 上层接入语义
- 重写轨迹生成、触发时序和 FANUC 实时控制链路
- 提供 Web 状态监控页面
- 在 Windows 和 Linux 上运行完整后台服务
说明:
- 这是长期运行的无头后台服务,不是 GUI 桌面程序。
- 第一版仅面向当前现场组合,后续再扩展机型与控制柜适配。
当前 Todo
- [x] 初始化独立仓库
- [x] 创建 `dotnet 8` 解决方案骨架
- [x] 打通最小宿主与 `/healthz`
- [x] 建立领域模型与模块边界
- [x] 落地配置兼容与机器人模型解析
- [ ] 落地轨迹规划、实时控制和 Web 状态页

View File

@@ -0,0 +1,13 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1,137 @@
namespace Flyshot.Core.Config;
/// <summary>
/// 标识需要生成哪一种平台风格的兼容路径。
/// </summary>
public enum CompatibilityPathStyle
{
/// <summary>
/// 使用 Linux/Unix 风格路径。
/// </summary>
Posix,
/// <summary>
/// 使用 Windows 风格路径。
/// </summary>
Windows
}
/// <summary>
/// 提供旧配置与新服务端之间的路径兼容策略。
/// </summary>
public static class PathCompatibility
{
/// <summary>
/// 按旧系统常见目录约定解析配置文件路径。
/// </summary>
/// <param name="configPath">调用方传入的原始配置路径。</param>
/// <param name="repoRoot">当前兼容搜索的仓库根目录。</param>
/// <returns>命中的绝对配置路径。</returns>
public static string ResolveConfigPath(string configPath, string repoRoot)
{
if (string.IsNullOrWhiteSpace(configPath))
{
throw new ArgumentException("配置路径不能为空。", nameof(configPath));
}
if (string.IsNullOrWhiteSpace(repoRoot))
{
throw new ArgumentException("仓库根目录不能为空。", nameof(repoRoot));
}
var rawPath = configPath.Trim();
if (Path.IsPathRooted(rawPath))
{
return File.Exists(rawPath)
? Path.GetFullPath(rawPath)
: throw new FileNotFoundException($"未找到配置文件: {rawPath}", rawPath);
}
var normalizedRepoRoot = Path.GetFullPath(repoRoot);
var fileName = Path.GetFileName(rawPath);
var checkedPaths = new List<string>();
// 先按最常见的候选路径顺序尝试,保持与旧工具链相近的定位逻辑。
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath, fileName))
{
var fullCandidate = Path.GetFullPath(candidate);
if (checkedPaths.Contains(fullCandidate, StringComparer.OrdinalIgnoreCase))
{
continue;
}
checkedPaths.Add(fullCandidate);
if (File.Exists(fullCandidate))
{
return fullCandidate;
}
}
// 最后一层兜底按文件名全仓库搜索,但只接受唯一命中,避免同名配置误判。
var matches = Directory
.EnumerateFiles(normalizedRepoRoot, fileName, SearchOption.AllDirectories)
.Select(Path.GetFullPath)
.Distinct(StringComparer.OrdinalIgnoreCase)
.ToArray();
if (matches.Length == 1)
{
return matches[0];
}
throw new FileNotFoundException(
$"未找到配置文件 '{configPath}'。已检查: {string.Join(", ", checkedPaths)}",
configPath);
}
/// <summary>
/// 构造当前平台约定的用户数据根目录。
/// </summary>
/// <param name="homeDirectory">用户主目录。</param>
/// <param name="pathStyle">目标平台风格。</param>
/// <returns>兼容旧系统的用户数据目录。</returns>
public static string BuildUserDataRoot(string homeDirectory, CompatibilityPathStyle pathStyle)
{
if (string.IsNullOrWhiteSpace(homeDirectory))
{
throw new ArgumentException("用户目录不能为空。", nameof(homeDirectory));
}
return pathStyle switch
{
CompatibilityPathStyle.Posix => JoinPosix(homeDirectory, ".Rvbust", "Data"),
CompatibilityPathStyle.Windows => JoinWindows(homeDirectory, ".Rvbust", "Data"),
_ => throw new ArgumentOutOfRangeException(nameof(pathStyle), pathStyle, "不支持的路径风格。")
};
}
/// <summary>
/// 枚举旧系统中最常见的配置候选路径。
/// </summary>
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath, string fileName)
{
yield return Path.Combine(repoRoot, rawPath);
yield return Path.Combine(repoRoot, "Rvbust", "Data", fileName);
yield return Path.Combine(repoRoot, "Rvbust", "Install", "FlyingShot", "Config", fileName);
yield return Path.Combine(repoRoot, "Rvbust", fileName);
yield return Path.Combine(repoRoot, fileName);
}
/// <summary>
/// 使用 Posix 风格拼接路径,便于在 Linux 下验证固定输出。
/// </summary>
private static string JoinPosix(string root, params string[] segments)
{
var trimmedRoot = root.TrimEnd('/');
return string.Join("/", new[] { trimmedRoot }.Concat(segments));
}
/// <summary>
/// 使用 Windows 风格拼接路径,避免在 Linux 上测试时被当前平台分隔符污染。
/// </summary>
private static string JoinWindows(string root, params string[] segments)
{
var normalizedRoot = root.TrimEnd('\\', '/').Replace('/', '\\');
return string.Join("\\", new[] { normalizedRoot }.Concat(segments));
}
}

View File

@@ -0,0 +1,278 @@
using System.Text.Json;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Config;
/// <summary>
/// 表示从旧版 RobotConfig.json 规范化后的机器人运行参数。
/// </summary>
public sealed class CompatibilityRobotSettings
{
/// <summary>
/// 初始化一份经过规范化的机器人兼容设置。
/// </summary>
public CompatibilityRobotSettings(
bool useDo,
IEnumerable<int> ioAddresses,
int ioKeepCycles,
double accLimitScale,
double jerkLimitScale,
int adaptIcspTryNum)
{
ArgumentNullException.ThrowIfNull(ioAddresses);
if (ioKeepCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(ioKeepCycles), "IO 保持周期不能为负数。");
}
if (accLimitScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
}
if (jerkLimitScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
}
if (adaptIcspTryNum < 0)
{
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
}
// 固化地址数组,避免上层代码在加载后原地篡改配置内容。
var copiedIoAddresses = ioAddresses.ToArray();
if (copiedIoAddresses.Any(address => address < 0))
{
throw new ArgumentException("IO 地址不能为负数。", nameof(ioAddresses));
}
UseDo = useDo;
IoAddresses = copiedIoAddresses;
IoKeepCycles = ioKeepCycles;
AccLimitScale = accLimitScale;
JerkLimitScale = jerkLimitScale;
AdaptIcspTryNum = adaptIcspTryNum;
}
/// <summary>
/// 获取是否启用伺服同步 IO。
/// </summary>
public bool UseDo { get; }
/// <summary>
/// 获取默认 IO 地址组。
/// </summary>
public IReadOnlyList<int> IoAddresses { get; }
/// <summary>
/// 获取单次触发保持的伺服周期数。
/// </summary>
public int IoKeepCycles { get; }
/// <summary>
/// 获取加速度全局倍率。
/// </summary>
public double AccLimitScale { get; }
/// <summary>
/// 获取 Jerk 全局倍率。
/// </summary>
public double JerkLimitScale { get; }
/// <summary>
/// 获取自适应补点最大尝试次数。
/// </summary>
public int AdaptIcspTryNum { get; }
}
/// <summary>
/// 表示一次旧版 RobotConfig.json 的完整加载结果。
/// </summary>
public sealed class LoadedRobotConfig
{
/// <summary>
/// 初始化一份规范化后的旧配置文档。
/// </summary>
public LoadedRobotConfig(
string sourcePath,
CompatibilityRobotSettings robot,
IReadOnlyDictionary<string, FlyshotProgram> programs)
{
if (string.IsNullOrWhiteSpace(sourcePath))
{
throw new ArgumentException("配置源路径不能为空。", nameof(sourcePath));
}
SourcePath = sourcePath;
Robot = robot ?? throw new ArgumentNullException(nameof(robot));
Programs = programs ?? throw new ArgumentNullException(nameof(programs));
}
/// <summary>
/// 获取实际命中的配置文件路径。
/// </summary>
public string SourcePath { get; }
/// <summary>
/// 获取机器人级兼容设置。
/// </summary>
public CompatibilityRobotSettings Robot { get; }
/// <summary>
/// 获取按名称索引的飞拍程序集合。
/// </summary>
public IReadOnlyDictionary<string, FlyshotProgram> Programs { get; }
}
/// <summary>
/// 负责读取旧版 RobotConfig.json 并转换成领域层可直接消费的结构。
/// </summary>
public sealed class RobotConfigLoader
{
/// <summary>
/// 加载一份旧版 RobotConfig.json。
/// </summary>
/// <param name="configPath">调用方传入的配置路径。</param>
/// <param name="repoRoot">用于兼容搜索的仓库根目录;为空时按当前工作目录推断。</param>
/// <returns>规范化后的配置文档。</returns>
public LoadedRobotConfig Load(string configPath, string? repoRoot = null)
{
var resolvedRepoRoot = ResolveRepoRoot(repoRoot);
var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot);
using var document = JsonDocument.Parse(File.ReadAllText(resolvedConfigPath));
var root = document.RootElement;
var robotElement = root.GetProperty("robot");
var flyingShotsElement = root.GetProperty("flying_shots");
var robot = new CompatibilityRobotSettings(
useDo: ReadBoolean(robotElement, "use_do", defaultValue: false),
ioAddresses: ReadIntArray(robotElement, "io_addr"),
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0));
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
foreach (var programElement in flyingShotsElement.EnumerateObject())
{
var programName = programElement.Name;
var program = ParseProgram(programName, programElement.Value);
programs.Add(programName, program);
}
return new LoadedRobotConfig(
sourcePath: resolvedConfigPath,
robot: robot,
programs: programs);
}
/// <summary>
/// 把单个 flying_shots 节点转成领域层 FlyshotProgram。
/// </summary>
private static FlyshotProgram ParseProgram(string programName, JsonElement programElement)
{
var waypointRows = programElement.GetProperty("traj_waypoints").EnumerateArray().ToArray();
var shotFlagElements = programElement.GetProperty("shot_flags").EnumerateArray().ToArray();
var waypoints = waypointRows
.Select(row => new JointWaypoint(row.EnumerateArray().Select(static value => value.GetDouble())))
.ToArray();
var shotFlags = shotFlagElements
.Select(static value => value.ValueKind == JsonValueKind.True || (value.ValueKind == JsonValueKind.Number && value.GetInt32() != 0))
.ToArray();
var waypointCount = waypoints.Length;
var offsetValues = programElement.TryGetProperty("offset_values", out var offsetValuesElement)
? offsetValuesElement.EnumerateArray().Select(static value => value.GetInt32()).ToArray()
: Enumerable.Repeat(0, waypointCount).ToArray();
var addressGroups = programElement.TryGetProperty("addr", out var addressElement)
? addressElement
.EnumerateArray()
.Select(group => new IoAddressGroup(group.EnumerateArray().Select(static value => value.GetInt32())))
.ToArray()
: Enumerable.Range(0, waypointCount)
.Select(_ => new IoAddressGroup(Array.Empty<int>()))
.ToArray();
return new FlyshotProgram(
name: programName,
waypoints: waypoints,
shotFlags: shotFlags,
offsetValues: offsetValues,
addressGroups: addressGroups);
}
/// <summary>
/// 解析布尔或 0/1 风格的兼容字段。
/// </summary>
private static bool ReadBoolean(JsonElement parent, string propertyName, bool defaultValue)
{
if (!parent.TryGetProperty(propertyName, out var property))
{
return defaultValue;
}
return property.ValueKind switch
{
JsonValueKind.True => true,
JsonValueKind.False => false,
JsonValueKind.Number => property.GetInt32() != 0,
_ => defaultValue
};
}
/// <summary>
/// 读取整数数组;字段不存在时返回空数组。
/// </summary>
private static IReadOnlyList<int> ReadIntArray(JsonElement parent, string propertyName)
{
return parent.TryGetProperty(propertyName, out var property)
? property.EnumerateArray().Select(static value => value.GetInt32()).ToArray()
: Array.Empty<int>();
}
/// <summary>
/// 读取整数兼容字段。
/// </summary>
private static int ReadInt(JsonElement parent, string propertyName, int defaultValue)
{
return parent.TryGetProperty(propertyName, out var property) ? property.GetInt32() : defaultValue;
}
/// <summary>
/// 读取浮点兼容字段。
/// </summary>
private static double ReadDouble(JsonElement parent, string propertyName, double defaultValue)
{
return parent.TryGetProperty(propertyName, out var property) ? property.GetDouble() : defaultValue;
}
/// <summary>
/// 推断仓库根目录,优先使用调用方显式传入的值。
/// </summary>
private static string ResolveRepoRoot(string? repoRoot)
{
if (!string.IsNullOrWhiteSpace(repoRoot))
{
return Path.GetFullPath(repoRoot);
}
var current = new DirectoryInfo(Directory.GetCurrentDirectory());
while (current is not null)
{
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
{
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
}
current = current.Parent;
}
return Directory.GetCurrentDirectory();
}
}

View File

@@ -0,0 +1,216 @@
using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Config;
/// <summary>
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
/// </summary>
public sealed class RobotModelLoader
{
private const uint JsonChunkType = 0x4E4F534A;
/// <summary>
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
/// </summary>
/// <param name="modelPath">.robot 文件路径。</param>
/// <param name="accLimitScale">加速度全局倍率。</param>
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
{
if (string.IsNullOrWhiteSpace(modelPath))
{
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
}
if (accLimitScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
}
if (jerkLimitScale <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
}
var resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText);
var robotBody = FindRobotBody(document.RootElement);
var profileName = robotBody.TryGetProperty("name", out var nameElement)
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
: Path.GetFileNameWithoutExtension(resolvedModelPath);
var jointLimits = new List<JointLimit>();
var jointCouplings = new List<JointCoupling>();
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
{
if (!IsPlanningJoint(jointElement))
{
continue;
}
var jointName = jointElement.GetProperty("name").GetString() ?? throw new InvalidDataException("关节缺少 name。");
var limitElement = jointElement.GetProperty("limit");
jointLimits.Add(new JointLimit(
jointName: jointName,
velocityLimit: limitElement.GetProperty("velocity").GetDouble(),
accelerationLimit: limitElement.GetProperty("acceleration").GetDouble() * accLimitScale,
jerkLimit: limitElement.GetProperty("jerk").GetDouble() * jerkLimitScale));
if (jointElement.TryGetProperty("couple", out var coupleElement))
{
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
jointCouplings.Add(new JointCoupling(
slaveJointName: jointName,
masterJointName: masterJointName,
multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
}
}
return new RobotProfile(
name: profileName,
modelPath: resolvedModelPath,
degreesOfFreedom: jointLimits.Count,
jointLimits: jointLimits,
jointCouplings: jointCouplings,
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
}
/// <summary>
/// 从 GLB 文件中提取 JSON chunk 文本。
/// </summary>
private static string ReadJsonChunk(string modelPath)
{
using var stream = File.OpenRead(modelPath);
using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false);
var magic = Encoding.ASCII.GetString(reader.ReadBytes(4));
if (!string.Equals(magic, "glTF", StringComparison.Ordinal))
{
throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。");
}
var version = reader.ReadUInt32();
if (version != 2)
{
throw new NotSupportedException($"当前仅支持 GLB 2.0,实际版本为 {version}。");
}
var totalLength = reader.ReadUInt32();
while (stream.Position < totalLength)
{
var chunkLength = reader.ReadUInt32();
var chunkType = reader.ReadUInt32();
var chunkBytes = reader.ReadBytes((int)chunkLength);
if (chunkType == JsonChunkType)
{
return Encoding.UTF8.GetString(chunkBytes);
}
}
throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。");
}
/// <summary>
/// 在 robotics.bodies 中找到 type=2 的机器人主体。
/// </summary>
private static JsonElement FindRobotBody(JsonElement root)
{
var bodies = root
.GetProperty("scenes")[0]
.GetProperty("extras")
.GetProperty("rvbust")
.GetProperty("robotics")
.GetProperty("bodies");
foreach (var body in bodies.EnumerateArray())
{
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
{
return body;
}
}
throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。");
}
/// <summary>
/// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
/// </summary>
/// <param name="modelPath">.robot 文件路径。</param>
/// <returns>包含完整关节几何链的运动学模型。</returns>
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
{
if (string.IsNullOrWhiteSpace(modelPath))
{
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
}
var resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText);
var robotBody = FindRobotBody(document.RootElement);
var profileName = robotBody.TryGetProperty("name", out var nameElement)
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
: Path.GetFileNameWithoutExtension(resolvedModelPath);
var joints = new List<RobotJointGeometry>();
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
{
var jointName = jointElement.GetProperty("name").GetString()
?? throw new InvalidDataException("关节缺少 name。");
var jointType = jointElement.TryGetProperty("type", out var typeElement)
? typeElement.GetInt32()
: 0;
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
// axis 字段有时存的是 4 元组 [x, y, z, scale],取最后 3 个作为方向向量。
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
var originXyz = origin.Length >= 3 ? origin[..3] : origin;
var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 };
string? coupleMaster = null;
double coupleMultiplier = 0.0;
double coupleOffset = 0.0;
if (jointElement.TryGetProperty("couple", out var coupleElement))
{
coupleMaster = coupleElement.GetProperty("master_joint").GetString();
coupleMultiplier = coupleElement.TryGetProperty("multiplier", out var m) ? m.GetDouble() : 0.0;
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
}
joints.Add(new RobotJointGeometry(
name: jointName,
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
jointType: jointType,
axis: axisVector,
originXyz: originXyz,
originQuatXyzw: originQuat,
coupleMaster: coupleMaster,
coupleMultiplier: coupleMultiplier,
coupleOffset: coupleOffset));
}
return new RobotKinematicsModel(name: profileName, joints: joints);
}
/// <summary>
/// 判断当前 joint 是否属于规划侧需要保留的旋转关节。
/// </summary>
private static bool IsPlanningJoint(JsonElement jointElement)
{
return jointElement.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2;
}
}

View File

@@ -0,0 +1,95 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Represents the current high-level controller state consumed by web monitoring and APIs.
/// </summary>
public sealed class ControllerStateSnapshot
{
/// <summary>
/// Initializes a controller state snapshot with safe default collections.
/// </summary>
public ControllerStateSnapshot(
DateTimeOffset capturedAt,
string connectionState,
bool isEnabled,
bool isInMotion,
double speedRatio,
IEnumerable<double>? jointPositions = null,
IEnumerable<double>? cartesianPose = null,
IEnumerable<RuntimeAlarm>? activeAlarms = null)
{
if (string.IsNullOrWhiteSpace(connectionState))
{
throw new ArgumentException("Connection state is required.", nameof(connectionState));
}
if (speedRatio < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "Speed ratio must be zero or positive.");
}
// Default to empty immutable snapshots so disconnected states still serialize safely.
var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty<double>();
var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty<double>();
var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty<RuntimeAlarm>();
CapturedAt = capturedAt;
ConnectionState = connectionState;
IsEnabled = isEnabled;
IsInMotion = isInMotion;
SpeedRatio = speedRatio;
JointPositions = copiedJointPositions;
CartesianPose = copiedCartesianPose;
ActiveAlarms = copiedActiveAlarms;
}
/// <summary>
/// Gets the capture time of the snapshot.
/// </summary>
[JsonPropertyName("capturedAt")]
public DateTimeOffset CapturedAt { get; }
/// <summary>
/// Gets the high-level connection state label.
/// </summary>
[JsonPropertyName("connectionState")]
public string ConnectionState { get; }
/// <summary>
/// Gets a value indicating whether the robot is enabled.
/// </summary>
[JsonPropertyName("isEnabled")]
public bool IsEnabled { get; }
/// <summary>
/// Gets a value indicating whether the robot is currently moving.
/// </summary>
[JsonPropertyName("isInMotion")]
public bool IsInMotion { get; }
/// <summary>
/// Gets the effective speed ratio reported by the controller.
/// </summary>
[JsonPropertyName("speedRatio")]
public double SpeedRatio { get; }
/// <summary>
/// Gets the current joint positions when available.
/// </summary>
[JsonPropertyName("jointPositions")]
public IReadOnlyList<double> JointPositions { get; }
/// <summary>
/// Gets the current Cartesian pose when available.
/// </summary>
[JsonPropertyName("cartesianPose")]
public IReadOnlyList<double> CartesianPose { get; }
/// <summary>
/// Gets the active alarms visible at capture time.
/// </summary>
[JsonPropertyName("activeAlarms")]
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
}

View File

@@ -0,0 +1,9 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
</Project>

View File

@@ -0,0 +1,175 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Represents an uploaded flyshot program built from teach points and shot metadata.
/// </summary>
public sealed class FlyshotProgram
{
/// <summary>
/// Initializes a validated flyshot program.
/// </summary>
public FlyshotProgram(
string name,
IEnumerable<JointWaypoint> waypoints,
IEnumerable<bool> shotFlags,
IEnumerable<int> offsetValues,
IEnumerable<IoAddressGroup> addressGroups)
{
if (string.IsNullOrWhiteSpace(name))
{
throw new ArgumentException("Program name is required.", nameof(name));
}
ArgumentNullException.ThrowIfNull(waypoints);
ArgumentNullException.ThrowIfNull(shotFlags);
ArgumentNullException.ThrowIfNull(offsetValues);
ArgumentNullException.ThrowIfNull(addressGroups);
// Freeze the uploaded data so later orchestration cannot silently drift from the request payload.
var copiedWaypoints = waypoints.ToArray();
var copiedShotFlags = shotFlags.ToArray();
var copiedOffsetValues = offsetValues.ToArray();
var copiedAddressGroups = addressGroups.ToArray();
if (copiedWaypoints.Length == 0)
{
throw new ArgumentException("At least one waypoint is required.", nameof(waypoints));
}
if (copiedShotFlags.Length != copiedWaypoints.Length)
{
throw new ArgumentException("Shot flag count must match waypoint count.", nameof(shotFlags));
}
if (copiedOffsetValues.Length != copiedWaypoints.Length)
{
throw new ArgumentException("Offset value count must match waypoint count.", nameof(offsetValues));
}
if (copiedAddressGroups.Length != copiedWaypoints.Length)
{
throw new ArgumentException("Address group count must match waypoint count.", nameof(addressGroups));
}
var jointDimension = copiedWaypoints[0].Positions.Count;
if (copiedWaypoints.Any(waypoint => waypoint.Positions.Count != jointDimension))
{
throw new ArgumentException("All waypoints must share the same joint dimension.", nameof(waypoints));
}
Name = name;
Waypoints = copiedWaypoints;
ShotFlags = copiedShotFlags;
OffsetValues = copiedOffsetValues;
AddressGroups = copiedAddressGroups;
JointDimension = jointDimension;
ShotWaypointCount = copiedShotFlags.Count(flag => flag);
}
/// <summary>
/// Gets the program name used by cache and gateway lookups.
/// </summary>
[JsonPropertyName("name")]
public string Name { get; }
/// <summary>
/// Gets the immutable teach waypoint list.
/// </summary>
[JsonPropertyName("waypoints")]
public IReadOnlyList<JointWaypoint> Waypoints { get; }
/// <summary>
/// Gets the per-waypoint shot trigger flags.
/// </summary>
[JsonPropertyName("shotFlags")]
public IReadOnlyList<bool> ShotFlags { get; }
/// <summary>
/// Gets the per-waypoint trigger offset values in servo cycles.
/// </summary>
[JsonPropertyName("offsetValues")]
public IReadOnlyList<int> OffsetValues { get; }
/// <summary>
/// Gets the per-waypoint IO address groups.
/// </summary>
[JsonPropertyName("addressGroups")]
public IReadOnlyList<IoAddressGroup> AddressGroups { get; }
/// <summary>
/// Gets the joint dimension shared by all waypoints.
/// </summary>
[JsonPropertyName("jointDimension")]
public int JointDimension { get; }
/// <summary>
/// Gets the number of waypoints that request a shot trigger.
/// </summary>
[JsonPropertyName("shotWaypointCount")]
public int ShotWaypointCount { get; }
}
/// <summary>
/// Represents a single teach waypoint in joint space.
/// </summary>
public sealed class JointWaypoint
{
/// <summary>
/// Initializes a validated joint waypoint.
/// </summary>
public JointWaypoint(IEnumerable<double> positions)
{
ArgumentNullException.ThrowIfNull(positions);
// Copy the input once so the waypoint remains immutable after upload.
var copiedPositions = positions.ToArray();
if (copiedPositions.Length == 0)
{
throw new ArgumentException("Joint waypoint must contain at least one joint value.", nameof(positions));
}
Positions = copiedPositions;
}
/// <summary>
/// Gets the immutable joint value vector.
/// </summary>
[JsonPropertyName("positions")]
public IReadOnlyList<double> Positions { get; }
}
/// <summary>
/// Represents the list of IO addresses that should fire at one logical trigger point.
/// </summary>
public sealed class IoAddressGroup
{
/// <summary>
/// Initializes a validated IO address group.
/// </summary>
public IoAddressGroup(IEnumerable<int> addresses)
{
ArgumentNullException.ThrowIfNull(addresses);
// Preserve address order because some upper-layer tooling expects exported order stability.
var copiedAddresses = addresses.ToArray();
if (copiedAddresses.Length > 8)
{
throw new ArgumentException("A single trigger point can contain at most eight IO addresses.", nameof(addresses));
}
if (copiedAddresses.Any(address => address < 0))
{
throw new ArgumentException("IO addresses must be zero or positive.", nameof(addresses));
}
Addresses = copiedAddresses;
}
/// <summary>
/// Gets the immutable ordered IO address list.
/// </summary>
[JsonPropertyName("addresses")]
public IReadOnlyList<int> Addresses { get; }
}

View File

@@ -0,0 +1,172 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// 描述机器人运动学链所需的完整关节几何信息,从 .robot GLB 中提取。
///
/// 为什么与 RobotProfile 分开?
/// ---
/// RobotProfile 只存规划侧需要的限速和 couple 元数据,是"规划约束视图"。
/// RobotKinematicsModel 存的是几何链origin、axis、变换顺序是"运动学视图"。
/// 两者生命周期和用途不同,分开可以避免规划层被迫依赖完整几何数据。
/// </summary>
public sealed class RobotKinematicsModel
{
/// <summary>
/// 初始化一份已验证的运动学模型。
/// </summary>
public RobotKinematicsModel(string name, IEnumerable<RobotJointGeometry> joints)
{
if (string.IsNullOrWhiteSpace(name))
{
throw new ArgumentException("机器人名称不能为空。", nameof(name));
}
ArgumentNullException.ThrowIfNull(joints);
var copiedJoints = joints.ToArray();
if (copiedJoints.Length == 0)
{
throw new ArgumentException("关节列表不能为空。", nameof(joints));
}
Name = name;
Joints = copiedJoints;
}
/// <summary>
/// 获取机器人名称。
/// </summary>
[JsonPropertyName("name")]
public string Name { get; }
/// <summary>
/// 获取按运动学链顺序排列的关节几何列表。
/// </summary>
[JsonPropertyName("joints")]
public IReadOnlyList<RobotJointGeometry> Joints { get; }
}
/// <summary>
/// 描述单个关节的几何属性,用于正运动学计算。
/// </summary>
public sealed class RobotJointGeometry
{
/// <summary>
/// 初始化一份已验证的关节几何描述。
/// </summary>
public RobotJointGeometry(
string name,
string parent,
string child,
int jointType,
double[] axis,
double[] originXyz,
double[] originQuatXyzw,
string? coupleMaster = null,
double coupleMultiplier = 0.0,
double coupleOffset = 0.0)
{
if (string.IsNullOrWhiteSpace(name))
{
throw new ArgumentException("关节名称不能为空。", nameof(name));
}
if (string.IsNullOrWhiteSpace(parent))
{
throw new ArgumentException("父节点名称不能为空。", nameof(parent));
}
if (string.IsNullOrWhiteSpace(child))
{
throw new ArgumentException("子节点名称不能为空。", nameof(child));
}
if (axis is null || axis.Length != 3)
{
throw new ArgumentException("关节轴必须是长度为 3 的数组。", nameof(axis));
}
if (originXyz is null || originXyz.Length != 3)
{
throw new ArgumentException("原点平移必须是长度为 3 的数组。", nameof(originXyz));
}
if (originQuatXyzw is null || originQuatXyzw.Length != 4)
{
throw new ArgumentException("原点旋转四元数必须是长度为 4 的数组xyzw。", nameof(originQuatXyzw));
}
Name = name;
Parent = parent;
Child = child;
JointType = jointType;
Axis = (double[])axis.Clone();
OriginXyz = (double[])originXyz.Clone();
OriginQuatXyzw = (double[])originQuatXyzw.Clone();
CoupleMaster = coupleMaster;
CoupleMultiplier = coupleMultiplier;
CoupleOffset = coupleOffset;
}
/// <summary>
/// 获取关节名称。
/// </summary>
[JsonPropertyName("name")]
public string Name { get; }
/// <summary>
/// 获取父连杆名称。
/// </summary>
[JsonPropertyName("parent")]
public string Parent { get; }
/// <summary>
/// 获取子连杆名称。
/// </summary>
[JsonPropertyName("child")]
public string Child { get; }
/// <summary>
/// 获取关节类型0=fixed, 1=prismatic, 2=revolute。
/// </summary>
[JsonPropertyName("jointType")]
public int JointType { get; }
/// <summary>
/// 获取关节旋转轴(单位向量)。
/// </summary>
[JsonPropertyName("axis")]
public double[] Axis { get; }
/// <summary>
/// 获取关节原点平移 [x, y, z]。
/// </summary>
[JsonPropertyName("originXyz")]
public double[] OriginXyz { get; }
/// <summary>
/// 获取关节原点旋转四元数 [x, y, z, w]。
/// </summary>
[JsonPropertyName("originQuatXyzw")]
public double[] OriginQuatXyzw { get; }
/// <summary>
/// 获取耦合主关节名称(如无则为 null
/// </summary>
[JsonPropertyName("coupleMaster")]
public string? CoupleMaster { get; }
/// <summary>
/// 获取耦合乘数。
/// </summary>
[JsonPropertyName("coupleMultiplier")]
public double CoupleMultiplier { get; }
/// <summary>
/// 获取耦合偏移。
/// </summary>
[JsonPropertyName("coupleOffset")]
public double CoupleOffset { get; }
}

View File

@@ -0,0 +1,226 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Describes the robot model contract consumed by planning and runtime orchestration.
/// </summary>
public sealed class RobotProfile
{
/// <summary>
/// Initializes a new robot profile with validated joint limits and coupling metadata.
/// </summary>
public RobotProfile(
string name,
string modelPath,
int degreesOfFreedom,
IEnumerable<JointLimit> jointLimits,
IEnumerable<JointCoupling> jointCouplings,
TimeSpan servoPeriod,
TimeSpan triggerPeriod)
{
if (string.IsNullOrWhiteSpace(name))
{
throw new ArgumentException("Robot profile name is required.", nameof(name));
}
if (string.IsNullOrWhiteSpace(modelPath))
{
throw new ArgumentException("Robot profile model path is required.", nameof(modelPath));
}
if (degreesOfFreedom <= 0)
{
throw new ArgumentOutOfRangeException(nameof(degreesOfFreedom), "Degrees of freedom must be positive.");
}
if (servoPeriod <= TimeSpan.Zero)
{
throw new ArgumentOutOfRangeException(nameof(servoPeriod), "Servo period must be positive.");
}
if (triggerPeriod <= TimeSpan.Zero)
{
throw new ArgumentOutOfRangeException(nameof(triggerPeriod), "Trigger period must be positive.");
}
ArgumentNullException.ThrowIfNull(jointLimits);
ArgumentNullException.ThrowIfNull(jointCouplings);
// Snapshot the collections once so downstream layers cannot mutate domain state in place.
var copiedJointLimits = jointLimits.ToArray();
var copiedJointCouplings = jointCouplings.ToArray();
if (copiedJointLimits.Length != degreesOfFreedom)
{
throw new ArgumentException("Joint limit count must match degrees of freedom.", nameof(jointLimits));
}
Name = name;
ModelPath = modelPath;
DegreesOfFreedom = degreesOfFreedom;
JointLimits = copiedJointLimits;
JointCouplings = copiedJointCouplings;
ServoPeriod = servoPeriod;
TriggerPeriod = triggerPeriod;
}
/// <summary>
/// Gets the robot profile name exposed to the rest of the runtime.
/// </summary>
[JsonPropertyName("name")]
public string Name { get; }
/// <summary>
/// Gets the source path of the robot model file.
/// </summary>
[JsonPropertyName("modelPath")]
public string ModelPath { get; }
/// <summary>
/// Gets the active revolute degree-of-freedom count.
/// </summary>
[JsonPropertyName("degreesOfFreedom")]
public int DegreesOfFreedom { get; }
/// <summary>
/// Gets the validated per-joint kinematic limits.
/// </summary>
[JsonPropertyName("jointLimits")]
public IReadOnlyList<JointLimit> JointLimits { get; }
/// <summary>
/// Gets optional joint coupling metadata parsed from the robot model.
/// </summary>
[JsonPropertyName("jointCouplings")]
public IReadOnlyList<JointCoupling> JointCouplings { get; }
/// <summary>
/// Gets the servo scheduling period used by the runtime.
/// </summary>
[JsonPropertyName("servoPeriod")]
public TimeSpan ServoPeriod { get; }
/// <summary>
/// Gets the trigger scheduling period used by shot-event alignment.
/// </summary>
[JsonPropertyName("triggerPeriod")]
public TimeSpan TriggerPeriod { get; }
}
/// <summary>
/// Describes a single revolute joint limit set required by the planners.
/// </summary>
public sealed class JointLimit
{
/// <summary>
/// Initializes a validated joint limit record.
/// </summary>
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
{
if (string.IsNullOrWhiteSpace(jointName))
{
throw new ArgumentException("Joint name is required.", nameof(jointName));
}
if (velocityLimit <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(velocityLimit), "Velocity limit must be positive.");
}
if (accelerationLimit <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(accelerationLimit), "Acceleration limit must be positive.");
}
if (jerkLimit <= 0.0)
{
throw new ArgumentOutOfRangeException(nameof(jerkLimit), "Jerk limit must be positive.");
}
JointName = jointName;
VelocityLimit = velocityLimit;
AccelerationLimit = accelerationLimit;
JerkLimit = jerkLimit;
}
/// <summary>
/// Gets the joint name associated with the limits.
/// </summary>
[JsonPropertyName("jointName")]
public string JointName { get; }
/// <summary>
/// Gets the velocity limit in joint space units.
/// </summary>
[JsonPropertyName("velocityLimit")]
public double VelocityLimit { get; }
/// <summary>
/// Gets the acceleration limit in joint space units.
/// </summary>
[JsonPropertyName("accelerationLimit")]
public double AccelerationLimit { get; }
/// <summary>
/// Gets the jerk limit in joint space units.
/// </summary>
[JsonPropertyName("jerkLimit")]
public double JerkLimit { get; }
}
/// <summary>
/// Describes a joint-coupling rule that must be applied before kinematics or planning.
/// </summary>
public sealed class JointCoupling
{
/// <summary>
/// Initializes a validated joint-coupling description.
/// </summary>
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
{
if (string.IsNullOrWhiteSpace(slaveJointName))
{
throw new ArgumentException("Slave joint name is required.", nameof(slaveJointName));
}
if (string.IsNullOrWhiteSpace(masterJointName))
{
throw new ArgumentException("Master joint name is required.", nameof(masterJointName));
}
if (string.Equals(slaveJointName, masterJointName, StringComparison.Ordinal))
{
throw new ArgumentException("Slave and master joints must be different.");
}
SlaveJointName = slaveJointName;
MasterJointName = masterJointName;
Multiplier = multiplier;
Offset = offset;
}
/// <summary>
/// Gets the dependent joint name.
/// </summary>
[JsonPropertyName("slaveJointName")]
public string SlaveJointName { get; }
/// <summary>
/// Gets the source joint name.
/// </summary>
[JsonPropertyName("masterJointName")]
public string MasterJointName { get; }
/// <summary>
/// Gets the coupling multiplier applied to the master joint angle.
/// </summary>
[JsonPropertyName("multiplier")]
public double Multiplier { get; }
/// <summary>
/// Gets the additive offset applied after the multiplier.
/// </summary>
[JsonPropertyName("offset")]
public double Offset { get; }
}

View File

@@ -0,0 +1,88 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Represents an alarm surfaced by planning, gateway, or runtime subsystems.
/// </summary>
public sealed class RuntimeAlarm
{
/// <summary>
/// Initializes a validated runtime alarm.
/// </summary>
public RuntimeAlarm(string code, string message, AlarmSeverity severity, DateTimeOffset raisedAt, bool isAcknowledged = false)
{
if (string.IsNullOrWhiteSpace(code))
{
throw new ArgumentException("Alarm code is required.", nameof(code));
}
if (string.IsNullOrWhiteSpace(message))
{
throw new ArgumentException("Alarm message is required.", nameof(message));
}
Code = code;
Message = message;
Severity = severity;
RaisedAt = raisedAt;
IsAcknowledged = isAcknowledged;
}
/// <summary>
/// Gets the stable alarm code.
/// </summary>
[JsonPropertyName("code")]
public string Code { get; }
/// <summary>
/// Gets the human-readable alarm message.
/// </summary>
[JsonPropertyName("message")]
public string Message { get; }
/// <summary>
/// Gets the alarm severity level.
/// </summary>
[JsonPropertyName("severity")]
public AlarmSeverity Severity { get; }
/// <summary>
/// Gets the time the alarm was raised.
/// </summary>
[JsonPropertyName("raisedAt")]
public DateTimeOffset RaisedAt { get; }
/// <summary>
/// Gets a value indicating whether the alarm has been acknowledged.
/// </summary>
[JsonPropertyName("isAcknowledged")]
public bool IsAcknowledged { get; }
}
/// <summary>
/// Identifies the severity level of a runtime alarm.
/// </summary>
[JsonConverter(typeof(JsonStringEnumConverter))]
public enum AlarmSeverity
{
/// <summary>
/// Informational alarm or notice.
/// </summary>
Info,
/// <summary>
/// Warning that does not yet block execution.
/// </summary>
Warning,
/// <summary>
/// Error that blocks the current operation.
/// </summary>
Error,
/// <summary>
/// Critical fault that requires immediate operator attention.
/// </summary>
Critical
}

View File

@@ -0,0 +1,71 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Describes the sampled shot-event mapping that downstream monitoring and reports consume.
/// </summary>
public sealed class ShotEvent
{
/// <summary>
/// Initializes a validated shot-event mapping result.
/// </summary>
public ShotEvent(int waypointIndex, double triggerTime, int sampleIndex, double sampleTime, IoAddressGroup addressGroup)
{
if (waypointIndex < 0)
{
throw new ArgumentOutOfRangeException(nameof(waypointIndex), "Waypoint index must be zero or positive.");
}
if (triggerTime < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(triggerTime), "Trigger time must be zero or positive.");
}
if (sampleIndex < 0)
{
throw new ArgumentOutOfRangeException(nameof(sampleIndex), "Sample index must be zero or positive.");
}
if (sampleTime < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(sampleTime), "Sample time must be zero or positive.");
}
WaypointIndex = waypointIndex;
TriggerTime = triggerTime;
SampleIndex = sampleIndex;
SampleTime = sampleTime;
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
}
/// <summary>
/// Gets the original teach-waypoint index.
/// </summary>
[JsonPropertyName("waypointIndex")]
public int WaypointIndex { get; }
/// <summary>
/// Gets the mathematically resolved trigger time.
/// </summary>
[JsonPropertyName("triggerTime")]
public double TriggerTime { get; }
/// <summary>
/// Gets the discrete sample index selected by the trigger scheduler.
/// </summary>
[JsonPropertyName("sampleIndex")]
public int SampleIndex { get; }
/// <summary>
/// Gets the sampled trigger time used by runtime outputs.
/// </summary>
[JsonPropertyName("sampleTime")]
public double SampleTime { get; }
/// <summary>
/// Gets the IO address group associated with the trigger.
/// </summary>
[JsonPropertyName("addressGroup")]
public IoAddressGroup AddressGroup { get; }
}

View File

@@ -0,0 +1,86 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Represents a request to plan or execute a flyshot trajectory.
/// </summary>
public sealed class TrajectoryRequest
{
/// <summary>
/// Initializes a trajectory request with compatibility-oriented defaults.
/// </summary>
public TrajectoryRequest(
RobotProfile robot,
FlyshotProgram program,
PlanningMethod method,
bool moveToStart = false,
bool saveTrajectoryArtifacts = false,
bool useCache = false)
{
Robot = robot ?? throw new ArgumentNullException(nameof(robot));
Program = program ?? throw new ArgumentNullException(nameof(program));
Method = method;
MoveToStart = moveToStart;
SaveTrajectoryArtifacts = saveTrajectoryArtifacts;
UseCache = useCache;
}
/// <summary>
/// Gets the robot profile that constrains planning and runtime compatibility.
/// </summary>
[JsonPropertyName("robot")]
public RobotProfile Robot { get; }
/// <summary>
/// Gets the uploaded program to be planned or executed.
/// </summary>
[JsonPropertyName("program")]
public FlyshotProgram Program { get; }
/// <summary>
/// Gets the selected planning method.
/// </summary>
[JsonPropertyName("method")]
public PlanningMethod Method { get; }
/// <summary>
/// Gets a value indicating whether the robot should move to the first waypoint before execution.
/// </summary>
[JsonPropertyName("moveToStart")]
public bool MoveToStart { get; }
/// <summary>
/// Gets a value indicating whether trajectory artifacts should be exported.
/// </summary>
[JsonPropertyName("saveTrajectoryArtifacts")]
public bool SaveTrajectoryArtifacts { get; }
/// <summary>
/// Gets a value indicating whether cached planning artifacts may be reused.
/// </summary>
[JsonPropertyName("useCache")]
public bool UseCache { get; }
}
/// <summary>
/// Identifies the planning mode requested by compatibility APIs.
/// </summary>
[JsonConverter(typeof(JsonStringEnumConverter))]
public enum PlanningMethod
{
/// <summary>
/// Uses the cubic-spline ICSP planner.
/// </summary>
Icsp,
/// <summary>
/// Uses the adaptive ICSP planner that may insert additional points.
/// </summary>
SelfAdaptIcsp,
/// <summary>
/// Uses the Double-S planner.
/// </summary>
Doubles
}

View File

@@ -0,0 +1,272 @@
using System.Text.Json.Serialization;
namespace Flyshot.Core.Domain;
/// <summary>
/// Represents the stable planning result returned to orchestration, SDKs, and monitoring layers.
/// </summary>
public sealed class TrajectoryResult
{
/// <summary>
/// Initializes a validated trajectory result.
/// </summary>
public TrajectoryResult(
string programName,
PlanningMethod method,
bool isValid,
TimeSpan duration,
IEnumerable<ShotEvent> shotEvents,
IEnumerable<TrajectoryDoEvent> triggerTimeline,
IEnumerable<TrajectoryArtifact> artifacts,
string? failureReason,
bool usedCache,
int originalWaypointCount,
int plannedWaypointCount)
{
if (string.IsNullOrWhiteSpace(programName))
{
throw new ArgumentException("Program name is required.", nameof(programName));
}
if (duration < TimeSpan.Zero)
{
throw new ArgumentOutOfRangeException(nameof(duration), "Duration must be zero or positive.");
}
if (originalWaypointCount < 0)
{
throw new ArgumentOutOfRangeException(nameof(originalWaypointCount), "Original waypoint count must be zero or positive.");
}
if (plannedWaypointCount < originalWaypointCount)
{
throw new ArgumentOutOfRangeException(nameof(plannedWaypointCount), "Planned waypoint count must be greater than or equal to the original waypoint count.");
}
ArgumentNullException.ThrowIfNull(shotEvents);
ArgumentNullException.ThrowIfNull(triggerTimeline);
ArgumentNullException.ThrowIfNull(artifacts);
// Materialize once so the result remains stable after the planner hands it off.
var copiedShotEvents = shotEvents.ToArray();
var copiedTriggerTimeline = triggerTimeline.ToArray();
var copiedArtifacts = artifacts.ToArray();
ProgramName = programName;
Method = method;
IsValid = isValid;
Duration = duration;
ShotEvents = copiedShotEvents;
TriggerTimeline = copiedTriggerTimeline;
Artifacts = copiedArtifacts;
FailureReason = failureReason;
UsedCache = usedCache;
OriginalWaypointCount = originalWaypointCount;
PlannedWaypointCount = plannedWaypointCount;
}
/// <summary>
/// Gets the source program name.
/// </summary>
[JsonPropertyName("programName")]
public string ProgramName { get; }
/// <summary>
/// Gets the method that produced the result.
/// </summary>
[JsonPropertyName("method")]
public PlanningMethod Method { get; }
/// <summary>
/// Gets a value indicating whether the result can be executed.
/// </summary>
[JsonPropertyName("isValid")]
public bool IsValid { get; }
/// <summary>
/// Gets the final trajectory duration.
/// </summary>
[JsonPropertyName("duration")]
public TimeSpan Duration { get; }
/// <summary>
/// Gets the sampled shot events exported for monitoring and reports.
/// </summary>
[JsonPropertyName("shotEvents")]
public IReadOnlyList<ShotEvent> ShotEvents { get; }
/// <summary>
/// Gets the trigger timeline that the runtime will inject into servo execution.
/// </summary>
[JsonPropertyName("triggerTimeline")]
public IReadOnlyList<TrajectoryDoEvent> TriggerTimeline { get; }
/// <summary>
/// Gets the exported trajectory artifacts associated with the result.
/// </summary>
[JsonPropertyName("artifacts")]
public IReadOnlyList<TrajectoryArtifact> Artifacts { get; }
/// <summary>
/// Gets the failure reason when the result is invalid.
/// </summary>
[JsonPropertyName("failureReason")]
public string? FailureReason { get; }
/// <summary>
/// Gets a value indicating whether the result reused cached planning data.
/// </summary>
[JsonPropertyName("usedCache")]
public bool UsedCache { get; }
/// <summary>
/// Gets the teach waypoint count before adaptive point insertion.
/// </summary>
[JsonPropertyName("originalWaypointCount")]
public int OriginalWaypointCount { get; }
/// <summary>
/// Gets the final waypoint count after planner preprocessing.
/// </summary>
[JsonPropertyName("plannedWaypointCount")]
public int PlannedWaypointCount { get; }
}
/// <summary>
/// Describes a trigger event that the runtime must inject into the servo timeline.
/// </summary>
public sealed class TrajectoryDoEvent
{
/// <summary>
/// Initializes a validated runtime trigger event.
/// </summary>
public TrajectoryDoEvent(int waypointIndex, double triggerTime, int offsetCycles, int holdCycles, IoAddressGroup addressGroup)
{
if (waypointIndex < 0)
{
throw new ArgumentOutOfRangeException(nameof(waypointIndex), "Waypoint index must be zero or positive.");
}
if (triggerTime < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(triggerTime), "Trigger time must be zero or positive.");
}
if (holdCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(holdCycles), "Hold cycles must be zero or positive.");
}
WaypointIndex = waypointIndex;
TriggerTime = triggerTime;
OffsetCycles = offsetCycles;
HoldCycles = holdCycles;
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
}
/// <summary>
/// Gets the original teach-waypoint index that requested the event.
/// </summary>
[JsonPropertyName("waypointIndex")]
public int WaypointIndex { get; }
/// <summary>
/// Gets the theoretical trigger time before discretization.
/// </summary>
[JsonPropertyName("triggerTime")]
public double TriggerTime { get; }
/// <summary>
/// Gets the configured offset in servo cycles.
/// </summary>
[JsonPropertyName("offsetCycles")]
public int OffsetCycles { get; }
/// <summary>
/// Gets the configured hold duration in servo cycles.
/// </summary>
[JsonPropertyName("holdCycles")]
public int HoldCycles { get; }
/// <summary>
/// Gets the IO address group to fire.
/// </summary>
[JsonPropertyName("addressGroup")]
public IoAddressGroup AddressGroup { get; }
}
/// <summary>
/// Describes an exported artifact generated from a planned trajectory.
/// </summary>
public sealed class TrajectoryArtifact
{
/// <summary>
/// Initializes a validated trajectory artifact descriptor.
/// </summary>
public TrajectoryArtifact(TrajectoryArtifactKind kind, string logicalName, string relativePath)
{
if (string.IsNullOrWhiteSpace(logicalName))
{
throw new ArgumentException("Logical artifact name is required.", nameof(logicalName));
}
if (string.IsNullOrWhiteSpace(relativePath))
{
throw new ArgumentException("Relative artifact path is required.", nameof(relativePath));
}
Kind = kind;
LogicalName = logicalName;
RelativePath = relativePath;
}
/// <summary>
/// Gets the exported artifact kind.
/// </summary>
[JsonPropertyName("kind")]
public TrajectoryArtifactKind Kind { get; }
/// <summary>
/// Gets the logical artifact file name.
/// </summary>
[JsonPropertyName("logicalName")]
public string LogicalName { get; }
/// <summary>
/// Gets the artifact path relative to the service output root.
/// </summary>
[JsonPropertyName("relativePath")]
public string RelativePath { get; }
}
/// <summary>
/// Identifies the exported trajectory artifact category.
/// </summary>
[JsonConverter(typeof(JsonStringEnumConverter))]
public enum TrajectoryArtifactKind
{
/// <summary>
/// Represents sparse joint teach points.
/// </summary>
JointTeachTrajectory,
/// <summary>
/// Represents sparse Cartesian teach points.
/// </summary>
CartesianTeachTrajectory,
/// <summary>
/// Represents dense joint trajectory samples.
/// </summary>
JointDenseTrajectory,
/// <summary>
/// Represents dense Cartesian trajectory samples.
/// </summary>
CartesianDenseTrajectory,
/// <summary>
/// Represents the exported shot-event mapping.
/// </summary>
ShotEventTimeline
}

View File

@@ -0,0 +1,298 @@
namespace Flyshot.Core.Planning;
/// <summary>
/// clamps 到零速度边界条件的三次样条插值器,专用于 ICSP 规划核心。
///
/// 为什么自己实现而不是用现成库?
/// ---
/// 逆向分析已经锁定原系统使用 scipy.interpolate.CubicSpline 的 clamped-zero 边界,
/// 且需要精确控制每段系数以便解析求导峰值。第三方库的边界条件语义和系数存储格式
/// 可能与 scipy 不完全对齐,自己实现可以把数学公式与 Python 参考代码逐行对照,
/// 确保后续 golden-sample 对拍时误差来源只集中在算法本身,而不是插值器差异。
/// </summary>
public sealed class CubicSplineInterpolator
{
private readonly double[] _times;
private readonly double[][] _values;
private readonly int _n; // 节点数
private readonly int _nseg; // 段数 = n - 1
private readonly int _dof; // 自由度(关节数)
// 每段每个关节的系数S_i(t) = a*t^3 + b*t^2 + c*t + dt 相对于段起点归一化到 [0, h]
// 数组维度 [_nseg][_dof]
private readonly double[][] _a;
private readonly double[][] _b;
private readonly double[][] _c;
private readonly double[][] _d;
private readonly double[] _h; // 每段时长
/// <summary>
/// 用 clamped-zero 边界构造三次样条。
/// </summary>
/// <param name="times">严格递增的时间节点,长度至少为 2。</param>
/// <param name="values">与 times 一一对应的路点值,每个路点是一个 double[dof]。</param>
/// <exception cref="ArgumentException">输入维度不一致或时间节点非递增时抛出。</exception>
public CubicSplineInterpolator(double[] times, double[][] values)
{
if (times.Length < 2)
{
throw new ArgumentException("时间节点至少需要 2 个。", nameof(times));
}
if (values.Length != times.Length)
{
throw new ArgumentException("路点数量必须与时间节点数量一致。", nameof(values));
}
_n = times.Length;
_nseg = _n - 1;
_dof = values[0].Length;
_times = new double[_n];
Array.Copy(times, _times, _n);
_values = new double[_n][];
for (int i = 0; i < _n; i++)
{
if (values[i].Length != _dof)
{
throw new ArgumentException($"第 {i} 个路点的维度与第一个路点不一致。");
}
_values[i] = new double[_dof];
Array.Copy(values[i], _values[i], _dof);
}
_h = new double[_nseg];
for (int i = 0; i < _nseg; i++)
{
_h[i] = _times[i + 1] - _times[i];
if (_h[i] <= 0.0)
{
throw new ArgumentException($"时间节点必须严格递增,但在索引 {i} 处发现 h={_h[i]}。");
}
}
_a = new double[_nseg][];
_b = new double[_nseg][];
_c = new double[_nseg][];
_d = new double[_nseg][];
for (int seg = 0; seg < _nseg; seg++)
{
_a[seg] = new double[_dof];
_b[seg] = new double[_dof];
_c[seg] = new double[_dof];
_d[seg] = new double[_dof];
}
BuildCoefficients();
}
/// <summary>
/// 获取路点数量。
/// </summary>
public int WaypointCount => _n;
/// <summary>
/// 获取关节自由度。
/// </summary>
public int DegreesOfFreedom => _dof;
/// <summary>
/// 获取时间节点数组的副本。
/// </summary>
public double[] Times
{
get
{
var copy = new double[_n];
Array.Copy(_times, copy, _n);
return copy;
}
}
/// <summary>
/// 在指定时刻求样条值(位置)。
/// </summary>
public double[] Evaluate(double t)
{
int seg = FindSegment(t);
double local = t - _times[seg];
double h = _h[seg];
var result = new double[_dof];
for (int d = 0; d < _dof; d++)
{
result[d] = _a[seg][d] * local * local * local
+ _b[seg][d] * local * local
+ _c[seg][d] * local
+ _d[seg][d];
}
return result;
}
/// <summary>
/// 在指定时刻求指定阶导数order=1 速度2 加速度3 jerk
/// </summary>
public double[] EvaluateDerivative(double t, int order)
{
if (order < 1 || order > 3)
{
throw new ArgumentOutOfRangeException(nameof(order), "仅支持 1~3 阶导数。");
}
int seg = FindSegment(t);
double local = t - _times[seg];
var result = new double[_dof];
for (int d = 0; d < _dof; d++)
{
result[d] = order switch
{
1 => 3.0 * _a[seg][d] * local * local + 2.0 * _b[seg][d] * local + _c[seg][d],
2 => 6.0 * _a[seg][d] * local + 2.0 * _b[seg][d],
3 => 6.0 * _a[seg][d],
_ => 0.0
};
}
return result;
}
/// <summary>
/// 解析计算每段每个关节的 1/2/3 阶导绝对值峰值。
///
/// 为什么用解析法而不是逐点采样?
/// ---
/// 三次样条的一阶导是二次函数、二阶导是线性函数、三阶导是常数,它们的极值点
/// 可以通过解析公式精确计算。逐点采样会引入"漏检"风险,解析法能保证找到真正的最大值。
/// </summary>
/// <returns>
/// 三元组:(max_dq[_nseg, _dof], max_ddq[_nseg, _dof], max_dddq[_nseg, _dof])。
/// </returns>
public (double[,] maxDq, double[,] maxDdq, double[,] maxDddq) SegmentMaxAbsDerivatives()
{
var maxDq = new double[_nseg, _dof];
var maxDdq = new double[_nseg, _dof];
var maxDddq = new double[_nseg, _dof];
for (int seg = 0; seg < _nseg; seg++)
{
double h = _h[seg];
for (int d = 0; d < _dof; d++)
{
double aa = _a[seg][d];
double bb = _b[seg][d];
double cc = _c[seg][d];
// 一阶导是二次函数 3a*t^2 + 2b*t + c峰值在端点或顶点。
var candidates = new List<double>(3)
{
Math.Abs(cc),
Math.Abs(3.0 * aa * h * h + 2.0 * bb * h + cc),
};
if (Math.Abs(aa) > 1e-12)
{
// 二次函数顶点:导数的导数 = 6a*t + 2b = 0 => t = -b/(3a)
double critical = -bb / (3.0 * aa);
if (critical > 0.0 && critical < h)
{
candidates.Add(Math.Abs(3.0 * aa * critical * critical + 2.0 * bb * critical + cc));
}
}
maxDq[seg, d] = candidates.Max();
// 二阶导是线性函数 6a*t + 2b最大值在端点。
maxDdq[seg, d] = Math.Max(Math.Abs(2.0 * bb), Math.Abs(6.0 * aa * h + 2.0 * bb));
// 三阶导是常数 6a。
maxDddq[seg, d] = Math.Abs(6.0 * aa);
}
}
return (maxDq, maxDdq, maxDddq);
}
/// <summary>
/// 定位给定时刻所属段索引,处理越界情况( clamp 到首尾段)。
/// </summary>
private int FindSegment(double t)
{
if (t <= _times[0])
{
return 0;
}
if (t >= _times[_n - 1])
{
return _nseg - 1;
}
// 线性扫描,因为路点数量通常很小(几十到几百个)。
for (int i = 0; i < _nseg; i++)
{
if (t >= _times[i] && t < _times[i + 1])
{
return i;
}
}
return _nseg - 1;
}
/// <summary>
/// 对每个关节独立建立 clamped-zero 边界的三次样条系数。
/// </summary>
private void BuildCoefficients()
{
for (int d = 0; d < _dof; d++)
{
// 解三对角方程组求二阶导数 M_i。
// 方程h_{i-1}*M_{i-1} + 2(h_{i-1}+h_i)*M_i + h_i*M_{i+1} = 6*((y_{i+1}-y_i)/h_i - (y_i-y_{i-1})/h_{i-1})
// 边界2h_0*M_0 + h_0*M_1 = 6*((y_1-y_0)/h_0 - y'_0)y'_0 = 0
// h_{n-2}*M_{n-2} + 2h_{n-2}*M_{n-1} = 6*(y'_n - (y_n-y_{n-1})/h_{n-2})y'_n = 0
var alpha = new double[_n];
alpha[0] = 6.0 * ((_values[1][d] - _values[0][d]) / _h[0] - 0.0);
alpha[_n - 1] = 6.0 * (0.0 - (_values[_n - 1][d] - _values[_n - 2][d]) / _h[_n - 2]);
for (int i = 1; i < _n - 1; i++)
{
alpha[i] = 6.0 * ((_values[i + 1][d] - _values[i][d]) / _h[i] - (_values[i][d] - _values[i - 1][d]) / _h[i - 1]);
}
var l = new double[_n];
var mu = new double[_n];
var z = new double[_n];
l[0] = 2.0 * _h[0];
mu[0] = 0.5;
z[0] = alpha[0] / l[0];
for (int i = 1; i < _n - 1; i++)
{
l[i] = 2.0 * (_h[i - 1] + _h[i]) - _h[i - 1] * mu[i - 1];
mu[i] = _h[i] / l[i];
z[i] = (alpha[i] - _h[i - 1] * z[i - 1]) / l[i];
}
l[_n - 1] = 2.0 * _h[_n - 2] - _h[_n - 2] * mu[_n - 2];
z[_n - 1] = (alpha[_n - 1] - _h[_n - 2] * z[_n - 2]) / l[_n - 1];
var m = new double[_n];
m[_n - 1] = z[_n - 1];
for (int j = _n - 2; j >= 0; j--)
{
m[j] = z[j] - mu[j] * m[j + 1];
}
// 计算每段系数
for (int seg = 0; seg < _nseg; seg++)
{
double h = _h[seg];
double yj = _values[seg][d];
double yj1 = _values[seg + 1][d];
_a[seg][d] = (m[seg + 1] - m[seg]) / (6.0 * h);
_b[seg][d] = m[seg] / 2.0;
_c[seg][d] = (yj1 - yj) / h - h * (2.0 * m[seg] + m[seg + 1]) / 6.0;
_d[seg][d] = yj;
}
}
}
}

View File

@@ -0,0 +1,66 @@
using System.Text;
using System.Text.Json;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Export;
/// <summary>
/// 将规划结果导出为与 Python demo 格式兼容的文本文件。
///
/// 为什么用空格分隔而不是 CSV
/// ---
/// 空格分隔是最简单的列对齐格式,不需要额外引号处理,也方便用 numpy.loadtxt
/// 或 C++ 的 fscanf 直接读取。真机侧的状态日志通常也是空格分隔的纯文本,
/// 保持一致便于后续做自动化 diff。
/// </summary>
public static class TrajectoryExporter
{
/// <summary>
/// 导出稠密关节轨迹到文本文件。
/// </summary>
public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
WriteDenseRows(path, rows);
}
/// <summary>
/// 导出稠密笛卡尔轨迹到文本文件。
/// </summary>
public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
WriteDenseRows(path, rows);
}
/// <summary>
/// 导出触发事件到 JSON 文件。
/// </summary>
public static void WriteShotEvents(string path, IReadOnlyList<ShotEvent> events)
{
var payload = events.Select(e => new
{
waypoint_index = e.WaypointIndex,
trigger_time = Math.Round(e.TriggerTime, 6),
sample_index = e.SampleIndex,
sample_time = Math.Round(e.SampleTime, 6),
addrs = e.AddressGroup.Addresses.ToList()
}).ToList();
var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions
{
WriteIndented = true
});
File.WriteAllText(path, json, new UTF8Encoding(false));
}
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
var sb = new StringBuilder();
foreach (var row in rows)
{
sb.AppendLine(string.Join(" ", row.Select(v => $"{v:F6}")));
}
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
}
}

View File

@@ -0,0 +1,13 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1,190 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning;
/// <summary>
/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。
///
/// 算法核心逻辑(与逆向文档一致):
/// 1. 初始段时长取关节空间欧氏距离;
/// 2. 每轮用当前时长构造 CubicSpline解析求每段 1/2/3 阶导峰值;
/// 3. 按速度一次方、加速度平方根、jerk 立方根缩放时长;
/// 4. 以 sum(|scale_i - 1|) 为收敛指标,保存历史最优结果;
/// 5. 最终用最优时长构造 CubicSpline 并输出时间轴。
/// </summary>
public sealed class ICspPlanner
{
/// <summary>
/// 默认收敛阈值,对应原实现中的 1e-3。
/// </summary>
public const double DefaultThreshold = 1e-3;
/// <summary>
/// 默认最大迭代轮数,对应原实现中的 1000。
/// </summary>
public const int DefaultMaxIterations = 1000;
/// <summary>
/// 执行 ICSP 规划,返回包含完整时间轴和收敛信息的轨迹。
/// </summary>
public PlannedTrajectory Plan(TrajectoryRequest request)
{
ArgumentNullException.ThrowIfNull(request);
var waypoints = request.Program.Waypoints;
if (waypoints.Count < 4)
{
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
}
var qs = WaypointsToArray(waypoints);
var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);
// 初始段时长直接取相邻示教点的关节空间欧氏距离。
var segmentDurations = ComputeInitialDurations(qs);
int nseg = segmentDurations.Length;
int dof = qs[0].Length;
double bestThreshold = double.PositiveInfinity;
double[]? bestDurations = null;
double[]? bestScales = null;
CubicSplineInterpolator? bestSpline = null;
int bestIterations = 0;
double[]? bestWaypointTimes = null;
for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++)
{
var waypointTimes = CumulativeTimes(segmentDurations);
var spline = new CubicSplineInterpolator(waypointTimes, qs);
var (maxDq, maxDdq, maxDddq) = spline.SegmentMaxAbsDerivatives();
var scales = new double[nseg];
for (int seg = 0; seg < nseg; seg++)
{
double segScale = 0.0;
for (int d = 0; d < dof; d++)
{
double sv = Math.Abs(maxDq[seg, d] / velLimits[d]);
double sa = Math.Sqrt(Math.Abs(maxDdq[seg, d] / accLimits[d]));
double sj = Math.Cbrt(Math.Abs(maxDddq[seg, d] / jerkLimits[d]));
segScale = Math.Max(segScale, Math.Max(sv, Math.Max(sa, sj)));
}
scales[seg] = segScale;
}
double currentThreshold = 0.0;
for (int seg = 0; seg < nseg; seg++)
{
currentThreshold += Math.Abs(scales[seg] - 1.0);
}
if (currentThreshold < bestThreshold)
{
bestThreshold = currentThreshold;
bestDurations = (double[])segmentDurations.Clone();
bestScales = (double[])scales.Clone();
bestSpline = spline;
bestIterations = iteration + 1;
bestWaypointTimes = (double[])waypointTimes.Clone();
}
if (currentThreshold < DefaultThreshold)
{
break;
}
for (int seg = 0; seg < nseg; seg++)
{
segmentDurations[seg] *= scales[seg];
}
}
if (bestSpline is null || bestDurations is null || bestScales is null || bestWaypointTimes is null)
{
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
}
return new PlannedTrajectory(
robot: request.Robot,
originalProgram: request.Program,
plannedWaypoints: waypoints,
waypointTimes: bestWaypointTimes,
segmentDurations: bestDurations,
segmentScales: bestScales,
method: PlanningMethod.Icsp,
iterations: bestIterations,
threshold: bestThreshold);
}
/// <summary>
/// 把领域层路点列表转换成 double[][],方便数学运算。
/// </summary>
private static double[][] WaypointsToArray(IReadOnlyList<JointWaypoint> waypoints)
{
var result = new double[waypoints.Count][];
for (int i = 0; i < waypoints.Count; i++)
{
result[i] = waypoints[i].Positions.ToArray();
}
return result;
}
/// <summary>
/// 从机器人配置中提取速度/加速度/jerk 限值数组。
/// </summary>
private static (double[] vel, double[] acc, double[] jerk) ExtractLimits(RobotProfile robot)
{
int dof = robot.DegreesOfFreedom;
var vel = new double[dof];
var acc = new double[dof];
var jerk = new double[dof];
for (int d = 0; d < dof; d++)
{
vel[d] = robot.JointLimits[d].VelocityLimit;
acc[d] = robot.JointLimits[d].AccelerationLimit;
jerk[d] = robot.JointLimits[d].JerkLimit;
}
return (vel, acc, jerk);
}
/// <summary>
/// 计算初始段时长:取相邻路点在关节空间的欧氏距离。
/// </summary>
private static double[] ComputeInitialDurations(double[][] qs)
{
int n = qs.Length;
var durations = new double[n - 1];
for (int i = 0; i < n - 1; i++)
{
double sumSq = 0.0;
for (int d = 0; d < qs[i].Length; d++)
{
double diff = qs[i + 1][d] - qs[i][d];
sumSq += diff * diff;
}
durations[i] = Math.Sqrt(sumSq);
}
return durations;
}
/// <summary>
/// 由段时长累加得到绝对时间节点(首项为 0
/// </summary>
private static double[] CumulativeTimes(double[] segmentDurations)
{
int nseg = segmentDurations.Length;
var times = new double[nseg + 1];
times[0] = 0.0;
for (int i = 0; i < nseg; i++)
{
times[i + 1] = times[i] + segmentDurations[i];
}
return times;
}
}

View File

@@ -0,0 +1,83 @@
namespace Flyshot.Core.Planning.Kinematics;
/// <summary>
/// 轻量 4x4 矩阵,用于机器人运动学计算,与 numpy 的 4x4 矩阵行为一致(列向量约定)。
/// </summary>
public struct Mat4
{
public double M11, M12, M13, M14;
public double M21, M22, M23, M24;
public double M31, M32, M33, M34;
public double M41, M42, M43, M44;
public static Mat4 Identity => new()
{
M11 = 1.0, M22 = 1.0, M33 = 1.0, M44 = 1.0
};
public static Mat4 operator *(Mat4 a, Mat4 b)
{
return new Mat4
{
M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31 + a.M14 * b.M41,
M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32 + a.M14 * b.M42,
M13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33 + a.M14 * b.M43,
M14 = a.M11 * b.M14 + a.M12 * b.M24 + a.M13 * b.M34 + a.M14 * b.M44,
M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31 + a.M24 * b.M41,
M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32 + a.M24 * b.M42,
M23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33 + a.M24 * b.M43,
M24 = a.M21 * b.M14 + a.M22 * b.M24 + a.M23 * b.M34 + a.M24 * b.M44,
M31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31 + a.M34 * b.M41,
M32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32 + a.M34 * b.M42,
M33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 + a.M34 * b.M43,
M34 = a.M31 * b.M14 + a.M32 * b.M24 + a.M33 * b.M34 + a.M34 * b.M44,
M41 = a.M41 * b.M11 + a.M42 * b.M21 + a.M43 * b.M31 + a.M44 * b.M41,
M42 = a.M41 * b.M12 + a.M42 * b.M22 + a.M43 * b.M32 + a.M44 * b.M42,
M43 = a.M41 * b.M13 + a.M42 * b.M23 + a.M43 * b.M33 + a.M44 * b.M43,
M44 = a.M41 * b.M14 + a.M42 * b.M24 + a.M43 * b.M34 + a.M44 * b.M44,
};
}
public static double[] operator *(Mat4 m, double[] v)
{
if (v.Length != 4)
throw new ArgumentException("向量长度必须为 4。", nameof(v));
return new double[]
{
m.M11 * v[0] + m.M12 * v[1] + m.M13 * v[2] + m.M14 * v[3],
m.M21 * v[0] + m.M22 * v[1] + m.M23 * v[2] + m.M24 * v[3],
m.M31 * v[0] + m.M32 * v[1] + m.M33 * v[2] + m.M34 * v[3],
m.M41 * v[0] + m.M42 * v[1] + m.M43 * v[2] + m.M44 * v[3],
};
}
/// <summary>
/// 从 3x3 旋转矩阵构造 4x4 齐次旋转矩阵(无平移)。
/// </summary>
public static Mat4 FromRotation(double[,] r)
{
return new Mat4
{
M11 = r[0, 0], M12 = r[0, 1], M13 = r[0, 2], M14 = 0.0,
M21 = r[1, 0], M22 = r[1, 1], M23 = r[1, 2], M24 = 0.0,
M31 = r[2, 0], M32 = r[2, 1], M33 = r[2, 2], M34 = 0.0,
M41 = 0.0, M42 = 0.0, M43 = 0.0, M44 = 1.0,
};
}
/// <summary>
/// 构造平移矩阵。
/// </summary>
public static Mat4 FromTranslation(double x, double y, double z)
{
var m = Identity;
m.M14 = x;
m.M24 = y;
m.M34 = z;
return m;
}
}

View File

@@ -0,0 +1,214 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Kinematics;
/// <summary>
/// 机器人正运动学计算,包含关节耦合解析、四元数/旋转矩阵转换和齐次变换链。
///
/// 为什么自己写而不是用 System.Numerics
/// ---
/// System.Numerics.Matrix4x4 使用行向量约定(平移在 M41-M43而 numpy 使用列向量
/// 约定(平移在 M14-M24-M34。两者是转置关系如果混用会导致 FK 结果完全错误。
/// 自己实现 Mat4 可以确保与 Python 参考代码的矩阵约定完全一致,便于 golden-sample 对拍。
/// </summary>
public static class RobotKinematics
{
/// <summary>
/// 执行正运动学,输出末端位姿 [x, y, z, qx, qy, qz, qw]。
/// </summary>
public static double[] ForwardKinematics(RobotKinematicsModel model, double[] jointPositions)
{
var kinematicAngles = ResolveKinematicAngles(model, jointPositions);
var transform = Mat4.Identity;
int revoluteIndex = 0;
foreach (var joint in model.Joints)
{
// 先叠加固定偏移 origin再叠加关节旋转。
transform = transform * TransformFromPose(joint.OriginXyz, joint.OriginQuatXyzw);
if (joint.JointType == 2)
{
var rot = AxisAngleToMatrix(joint.Axis, kinematicAngles[revoluteIndex]);
var jointTransform = Mat4.FromRotation(rot);
transform = transform * jointTransform;
revoluteIndex++;
}
}
var quat = MatrixToQuatXyzw(ExtractRotation(transform));
return new double[]
{
transform.M14, transform.M24, transform.M34,
quat[0], quat[1], quat[2], quat[3]
};
}
/// <summary>
/// 解析关节耦合,把控制器关节角转换成运动学关节角。
///
/// 公式q_kin[i] = q_raw[i] + multiplier * q_kin[master] + offset
/// </summary>
public static double[] ResolveKinematicAngles(RobotKinematicsModel model, double[] rawPositions)
{
var revoluteJoints = model.Joints.Where(j => j.JointType == 2).ToArray();
if (rawPositions.Length != revoluteJoints.Length)
{
throw new ArgumentException(
$"关节位置数量 {rawPositions.Length} 与旋转关节数量 {revoluteJoints.Length} 不匹配。");
}
var nameToIndex = revoluteJoints
.Select((j, i) => (j.Name, i))
.ToDictionary(x => x.Name, x => x.i);
var kinematicValues = new double[rawPositions.Length];
var resolved = new bool[rawPositions.Length];
var resolving = new bool[rawPositions.Length];
double Resolve(int index)
{
if (resolved[index])
return kinematicValues[index];
if (resolving[index])
throw new InvalidOperationException($"检测到循环关节耦合:{revoluteJoints[index].Name}");
resolving[index] = true;
var joint = revoluteJoints[index];
double value = rawPositions[index];
if (!string.IsNullOrEmpty(joint.CoupleMaster))
{
if (!nameToIndex.TryGetValue(joint.CoupleMaster, out var masterIndex))
throw new InvalidOperationException($"关节 {joint.Name} 引用了未知的主关节 {joint.CoupleMaster}");
value += joint.CoupleMultiplier * Resolve(masterIndex) + joint.CoupleOffset;
}
kinematicValues[index] = value;
resolving[index] = false;
resolved[index] = true;
return value;
}
for (int i = 0; i < revoluteJoints.Length; i++)
{
Resolve(i);
}
return kinematicValues;
}
/// <summary>
/// 从 Mat4 中提取 3x3 旋转矩阵。
/// </summary>
private static double[,] ExtractRotation(Mat4 m)
{
return new double[,]
{
{ m.M11, m.M12, m.M13 },
{ m.M21, m.M22, m.M23 },
{ m.M31, m.M32, m.M33 },
};
}
/// <summary>
/// 把平移 + 四元数拼成 4x4 齐次变换矩阵。
/// </summary>
private static Mat4 TransformFromPose(double[] xyz, double[] quatXyzw)
{
var rot = QuatToMatrix(quatXyzw);
var m = Mat4.Identity;
m.M11 = rot[0, 0]; m.M12 = rot[0, 1]; m.M13 = rot[0, 2];
m.M21 = rot[1, 0]; m.M22 = rot[1, 1]; m.M23 = rot[1, 2];
m.M31 = rot[2, 0]; m.M32 = rot[2, 1]; m.M33 = rot[2, 2];
m.M14 = xyz[0];
m.M24 = xyz[1];
m.M34 = xyz[2];
return m;
}
/// <summary>
/// 四元数xyzw 顺序)转旋转矩阵。
/// </summary>
private static double[,] QuatToMatrix(double[] q)
{
double x = q[0], y = q[1], z = q[2], w = q[3];
double xx = x * x, yy = y * y, zz = z * z;
double xy = x * y, xz = x * z, yz = y * z;
double wx = w * x, wy = w * y, wz = w * z;
return new double[,]
{
{ 1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy) },
{ 2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx) },
{ 2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy) },
};
}
/// <summary>
/// 旋转矩阵转四元数xyzw 顺序),使用 Shepperd 分支策略保证数值稳定。
/// </summary>
private static double[] MatrixToQuatXyzw(double[,] r)
{
double trace = r[0, 0] + r[1, 1] + r[2, 2];
double x, y, z, w;
if (trace > 0.0)
{
double s = Math.Sqrt(trace + 1.0) * 2.0;
w = 0.25 * s;
x = (r[2, 1] - r[1, 2]) / s;
y = (r[0, 2] - r[2, 0]) / s;
z = (r[1, 0] - r[0, 1]) / s;
}
else if (r[0, 0] > r[1, 1] && r[0, 0] > r[2, 2])
{
double s = Math.Sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0;
w = (r[2, 1] - r[1, 2]) / s;
x = 0.25 * s;
y = (r[0, 1] + r[1, 0]) / s;
z = (r[0, 2] + r[2, 0]) / s;
}
else if (r[1, 1] > r[2, 2])
{
double s = Math.Sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0;
w = (r[0, 2] - r[2, 0]) / s;
x = (r[0, 1] + r[1, 0]) / s;
y = 0.25 * s;
z = (r[1, 2] + r[2, 1]) / s;
}
else
{
double s = Math.Sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0;
w = (r[1, 0] - r[0, 1]) / s;
x = (r[0, 2] + r[2, 0]) / s;
y = (r[1, 2] + r[2, 1]) / s;
z = 0.25 * s;
}
double norm = Math.Sqrt(x * x + y * y + z * z + w * w);
return new double[] { x / norm, y / norm, z / norm, w / norm };
}
/// <summary>
/// Rodrigues 公式:绕单位向量 axis 旋转 angle 弧度。
/// </summary>
private static double[,] AxisAngleToMatrix(double[] axis, double angle)
{
double norm = Math.Sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
if (norm == 0.0)
return new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
double ux = axis[0] / norm, uy = axis[1] / norm, uz = axis[2] / norm;
double c = Math.Cos(angle);
double s = Math.Sin(angle);
double oneC = 1.0 - c;
return new double[,]
{
{ c + ux * ux * oneC, ux * uy * oneC - uz * s, ux * uz * oneC + uy * s },
{ uy * ux * oneC + uz * s, c + uy * uy * oneC, uy * uz * oneC - ux * s },
{ uz * ux * oneC - uy * s, uz * uy * oneC + ux * s, c + uz * uz * oneC },
};
}
}

View File

@@ -0,0 +1,130 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning;
/// <summary>
/// 表示一次规划完成后的轨迹结果,包含原始示教点、补中点后的完整路点、时间轴和收敛信息。
/// </summary>
public sealed class PlannedTrajectory
{
/// <summary>
/// 初始化一份已验证的规划轨迹。
/// </summary>
public PlannedTrajectory(
RobotProfile robot,
FlyshotProgram originalProgram,
IReadOnlyList<JointWaypoint> plannedWaypoints,
IReadOnlyList<double> waypointTimes,
IReadOnlyList<double> segmentDurations,
IReadOnlyList<double> segmentScales,
PlanningMethod method,
int iterations,
double threshold)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(originalProgram);
ArgumentNullException.ThrowIfNull(plannedWaypoints);
ArgumentNullException.ThrowIfNull(waypointTimes);
ArgumentNullException.ThrowIfNull(segmentDurations);
ArgumentNullException.ThrowIfNull(segmentScales);
if (waypointTimes.Count != plannedWaypoints.Count)
{
throw new ArgumentException("路点时间轴长度必须与路点数量一致。", nameof(waypointTimes));
}
if (segmentDurations.Count != plannedWaypoints.Count - 1)
{
throw new ArgumentException("段时长数量必须等于路点数量减一。", nameof(segmentDurations));
}
if (segmentScales.Count != segmentDurations.Count)
{
throw new ArgumentException("段缩放因子数量必须与段时长数量一致。", nameof(segmentScales));
}
if (iterations < 0)
{
throw new ArgumentOutOfRangeException(nameof(iterations), "迭代次数不能为负数。");
}
if (threshold < 0.0)
{
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值不能为负数。");
}
Robot = robot;
OriginalProgram = originalProgram;
PlannedWaypoints = plannedWaypoints;
WaypointTimes = waypointTimes;
SegmentDurations = segmentDurations;
SegmentScales = segmentScales;
Method = method;
Iterations = iterations;
Threshold = threshold;
OriginalWaypointCount = originalProgram.Waypoints.Count;
InsertedWaypointCount = plannedWaypoints.Count - OriginalWaypointCount;
PlannedWaypointCount = plannedWaypoints.Count;
}
/// <summary>
/// 获取规划时使用的机器人配置文件。
/// </summary>
public RobotProfile Robot { get; }
/// <summary>
/// 获取原始上传的飞拍程序含示教点、shot_flags 等)。
/// </summary>
public FlyshotProgram OriginalProgram { get; }
/// <summary>
/// 获取规划后完整的路点列表(可能已补中点)。
/// </summary>
public IReadOnlyList<JointWaypoint> PlannedWaypoints { get; }
/// <summary>
/// 获取每个路点对应的绝对时间(单位:秒)。
/// </summary>
public IReadOnlyList<double> WaypointTimes { get; }
/// <summary>
/// 获取每段的规划时长(单位:秒)。
/// </summary>
public IReadOnlyList<double> SegmentDurations { get; }
/// <summary>
/// 获取每段最终的速度/加速度/jerk 缩放因子。
/// </summary>
public IReadOnlyList<double> SegmentScales { get; }
/// <summary>
/// 获取本次规划使用的方法。
/// </summary>
public PlanningMethod Method { get; }
/// <summary>
/// 获取 ICSP 实际执行的迭代轮数。
/// </summary>
public int Iterations { get; }
/// <summary>
/// 获取最终收敛指标sum(|scale_i - 1|))。
/// </summary>
public double Threshold { get; }
/// <summary>
/// 获取原始示教点数量(不含补中点)。
/// </summary>
public int OriginalWaypointCount { get; }
/// <summary>
/// 获取补中点插入的数量。
/// </summary>
public int InsertedWaypointCount { get; }
/// <summary>
/// 获取规划后的总路点数量(含补中点)。
/// </summary>
public int PlannedWaypointCount { get; }
}

View File

@@ -0,0 +1,106 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Kinematics;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 从规划轨迹生成稠密关节/笛卡尔采样序列。
///
/// 为什么采样周期默认 0.016s
/// ---
/// 真机伺服周期是 8ms125Hz但规划层通常以 16ms约 60Hz输出到上层。
/// 0.016 是一个经验默认值,与真机观测到的稠密轨迹间隔一致。
/// </summary>
public static class TrajectorySampler
{
/// <summary>
/// 生成稠密关节轨迹采样,每行格式:[time, j1, j2, ..., jN],保留 6 位小数。
/// </summary>
public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
PlannedTrajectory trajectory,
double samplePeriod = 0.016,
int decimals = 6)
{
var spline = RebuildSpline(trajectory);
double duration = trajectory.WaypointTimes[^1];
var times = GenerateSampleTimes(duration, samplePeriod);
var result = new List<IReadOnlyList<double>>(times.Count);
foreach (var t in times)
{
var pos = spline.Evaluate(t);
var row = new List<double>(pos.Length + 1);
row.Add(Math.Round(t, decimals));
foreach (var value in pos)
{
row.Add(Math.Round(value, decimals));
}
result.Add(row);
}
return result;
}
/// <summary>
/// 生成稠密笛卡尔轨迹采样,每行格式:[time, x, y, z, qx, qy, qz, qw],保留 6 位小数。
/// </summary>
public static IReadOnlyList<IReadOnlyList<double>> SampleCartesianTrajectory(
PlannedTrajectory trajectory,
RobotKinematicsModel kinematicsModel,
double samplePeriod = 0.016,
int decimals = 6)
{
var spline = RebuildSpline(trajectory);
double duration = trajectory.WaypointTimes[^1];
var times = GenerateSampleTimes(duration, samplePeriod);
var result = new List<IReadOnlyList<double>>(times.Count);
foreach (var t in times)
{
var jointPos = spline.Evaluate(t);
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
var row = new List<double>(pose.Length + 1);
row.Add(Math.Round(t, decimals));
foreach (var value in pose)
{
row.Add(Math.Round(value, decimals));
}
result.Add(row);
}
return result;
}
/// <summary>
/// 从规划轨迹重建 CubicSplineInterpolator 用于稠密采样。
/// </summary>
private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
{
var times = trajectory.WaypointTimes.ToArray();
var values = trajectory.PlannedWaypoints.Select(wp => wp.Positions.ToArray()).ToArray();
return new CubicSplineInterpolator(times, values);
}
/// <summary>
/// 生成采样时间点,强制包含终点。
/// </summary>
private static IReadOnlyList<double> GenerateSampleTimes(double duration, double samplePeriod)
{
var times = new List<double>();
double t = 0.0;
while (t < duration - 1e-12)
{
times.Add(t);
t += samplePeriod;
}
if (times.Count == 0 || Math.Abs(times[^1] - duration) > 1e-12)
{
times.Add(duration);
}
return times;
}
}

View File

@@ -0,0 +1,182 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning;
/// <summary>
/// 在 ICSP 外层包裹补中点策略,实现 self-adapt-icsp 行为。
///
/// 为什么需要这层?
/// ---
/// 逆向分析已经指出:原系统里普通 icsp 若仍有段 scale > 1不会直接返回未收敛结果
/// 配置中还明确存在 adapt_icsp_try_num。本层把“超限段统一插入中点后再重规划”的逻辑显式落地
/// 补上 demo 缺失的失败恢复路径。
///
/// 补点策略:
/// ---
/// 对当前所有 scale > 1 + tolerance 的段统一插入关节空间中点,然后把新路点集交给 ICSPPlanner
/// 重新规划。这种"先把明显病灶都降一档,再整体重规划"的策略比逐段拆分更稳定,
/// 也更符合服务端 adapt_icsp_try_num 的意图。
/// </summary>
public sealed class SelfAdaptIcspPlanner
{
/// <summary>
/// 判定段是否超限的数值容差,过滤浮点噪声。
/// </summary>
public const double ScaleTolerance = 5e-4;
private readonly ICspPlanner _innerPlanner = new();
/// <summary>
/// 执行自适应 ICSP 规划,允许在超限段插入中点后重试。
/// </summary>
/// <param name="request">轨迹规划请求。</param>
/// <param name="adaptIcspTryNum">最大补点重试次数(默认 5。</param>
/// <returns>规划后的轨迹结果。</returns>
/// <exception cref="InvalidOperationException">超过最大重试次数仍未收敛时抛出。</exception>
public PlannedTrajectory Plan(TrajectoryRequest request, int adaptIcspTryNum = 5)
{
ArgumentNullException.ThrowIfNull(request);
var currentWaypoints = request.Program.Waypoints.ToArray();
var currentShotFlags = request.Program.ShotFlags.ToArray();
var currentOffsets = request.Program.OffsetValues.ToArray();
var currentAddrs = request.Program.AddressGroups.ToArray();
int originalWaypointCount = currentWaypoints.Length;
if (originalWaypointCount < 4)
{
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
}
var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
var currentRequest = new TrajectoryRequest(
robot: request.Robot,
program: currentProgram,
method: PlanningMethod.SelfAdaptIcsp,
moveToStart: request.MoveToStart,
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
useCache: request.UseCache);
PlannedTrajectory? lastTrajectory = null;
int maxAttempts = Math.Max(0, adaptIcspTryNum);
for (int attempt = 0; attempt <= maxAttempts; attempt++)
{
var trajectory = _innerPlanner.Plan(currentRequest);
lastTrajectory = trajectory;
var badSegments = new List<int>();
for (int seg = 0; seg < trajectory.SegmentScales.Count; seg++)
{
if (trajectory.SegmentScales[seg] > 1.0 + ScaleTolerance)
{
badSegments.Add(seg);
}
}
if (badSegments.Count == 0)
{
// 所有段都满足约束,收敛成功。返回包含补中点后路点的轨迹。
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: request.Program,
plannedWaypoints: currentWaypoints,
waypointTimes: trajectory.WaypointTimes,
segmentDurations: trajectory.SegmentDurations,
segmentScales: trajectory.SegmentScales,
method: PlanningMethod.SelfAdaptIcsp,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
if (attempt >= maxAttempts)
{
break;
}
// 对超限段插入中点,并同步扩展 shot 元数据。
(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);
currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
currentRequest = new TrajectoryRequest(
robot: request.Robot,
program: currentProgram,
method: PlanningMethod.SelfAdaptIcsp,
moveToStart: request.MoveToStart,
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
useCache: request.UseCache);
}
double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
throw new InvalidOperationException(
$"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛,最大段缩放因子={maxScale:F6}。");
}
/// <summary>
/// 用当前路点集和元数据构造 FlyshotProgram。
/// </summary>
private static FlyshotProgram BuildProgram(
string name,
JointWaypoint[] waypoints,
bool[] shotFlags,
int[] offsets,
IoAddressGroup[] addrs)
{
return new FlyshotProgram(
name: name,
waypoints: waypoints,
shotFlags: shotFlags,
offsetValues: offsets,
addressGroups: addrs);
}
/// <summary>
/// 对超限段统一插入关节空间中点,并同步扩展 shot 元数据。
/// 新插入的路点默认 shotFlag=false、offset=0、addr=空。
/// </summary>
private static (JointWaypoint[] waypoints, bool[] shotFlags, int[] offsets, IoAddressGroup[] addrs)
InsertSegmentMidpoints(
JointWaypoint[] waypoints,
bool[] shotFlags,
int[] offsets,
IoAddressGroup[] addrs,
List<int> badSegments)
{
if (badSegments.Count == 0)
{
return (waypoints, shotFlags, offsets, addrs);
}
var badSet = new HashSet<int>(badSegments);
var newWaypoints = new List<JointWaypoint>(waypoints.Length + badSegments.Count);
var newShotFlags = new List<bool>(waypoints.Length + badSegments.Count);
var newOffsets = new List<int>(waypoints.Length + badSegments.Count);
var newAddrs = new List<IoAddressGroup>(waypoints.Length + badSegments.Count);
newWaypoints.Add(waypoints[0]);
newShotFlags.Add(shotFlags[0]);
newOffsets.Add(offsets[0]);
newAddrs.Add(addrs[0]);
for (int seg = 0; seg < waypoints.Length - 1; seg++)
{
if (badSet.Contains(seg))
{
var mid = new JointWaypoint(
waypoints[seg].Positions.Zip(waypoints[seg + 1].Positions, (a, b) => (a + b) / 2.0));
newWaypoints.Add(mid);
newShotFlags.Add(false);
newOffsets.Add(0);
newAddrs.Add(new IoAddressGroup(Array.Empty<int>()));
}
newWaypoints.Add(waypoints[seg + 1]);
newShotFlags.Add(shotFlags[seg + 1]);
newOffsets.Add(offsets[seg + 1]);
newAddrs.Add(addrs[seg + 1]);
}
return (newWaypoints.ToArray(), newShotFlags.ToArray(), newOffsets.ToArray(), newAddrs.ToArray());
}
}

View File

@@ -0,0 +1,14 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1,31 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Triggering;
/// <summary>
/// 表示一次飞拍触发时间轴的完整计算结果,包含理论触发事件和运行时注入事件。
/// </summary>
public sealed class ShotTimeline
{
/// <summary>
/// 初始化一份已验证的触发时间轴。
/// </summary>
public ShotTimeline(IEnumerable<ShotEvent> shotEvents, IEnumerable<TrajectoryDoEvent> triggerTimeline)
{
ArgumentNullException.ThrowIfNull(shotEvents);
ArgumentNullException.ThrowIfNull(triggerTimeline);
ShotEvents = shotEvents.ToArray();
TriggerTimeline = triggerTimeline.ToArray();
}
/// <summary>
/// 获取所有拍照触发事件(含理论和离散化时间)。
/// </summary>
public IReadOnlyList<ShotEvent> ShotEvents { get; }
/// <summary>
/// 获取用于运行时伺服流注入的 DO 事件列表。
/// </summary>
public IReadOnlyList<TrajectoryDoEvent> TriggerTimeline { get; }
}

View File

@@ -0,0 +1,82 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Triggering;
/// <summary>
/// 根据规划轨迹和飞拍配置生成触发时间轴,把示教点上的 shot_flags / offset_values / addr
/// 映射成带理论时间和离散化时间的 ShotEvent以及可直接注入伺服流的 TrajectoryDoEvent。
/// </summary>
public sealed class ShotTimelineBuilder
{
private readonly WaypointTimestampResolver _resolver;
/// <summary>
/// 初始化 ShotTimelineBuilder依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
/// </summary>
public ShotTimelineBuilder(WaypointTimestampResolver resolver)
{
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
}
/// <summary>
/// 为给定轨迹构建完整的触发时间轴。
/// </summary>
/// <param name="trajectory">规划后的轨迹(含补中点信息和机器人配置)。</param>
/// <param name="holdCycles">IO 保持周期数(对应原系统的 io_keep_cycles。</param>
/// <param name="samplePeriod">稠密采样周期,用于离散化 sample_index 和 sample_time。</param>
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod)
{
ArgumentNullException.ThrowIfNull(trajectory);
if (holdCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(holdCycles), "IO 保持周期数不能为负数。");
}
if (samplePeriod <= TimeSpan.Zero)
{
throw new ArgumentOutOfRangeException(nameof(samplePeriod), "采样周期必须大于零。");
}
var timestamps = _resolver.Resolve(trajectory);
var program = trajectory.OriginalProgram;
var robot = trajectory.Robot;
double triggerPeriodSeconds = robot.TriggerPeriod.TotalSeconds;
double samplePeriodSeconds = samplePeriod.TotalSeconds;
var shotEvents = new List<ShotEvent>();
var triggerTimeline = new List<TrajectoryDoEvent>();
for (int i = 0; i < program.Waypoints.Count; i++)
{
if (!program.ShotFlags[i])
{
continue;
}
double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
double sampleTime = sampleIndex * samplePeriodSeconds;
var addressGroup = program.AddressGroups[i];
shotEvents.Add(new ShotEvent(
waypointIndex: i,
triggerTime: triggerTime,
sampleIndex: sampleIndex,
sampleTime: sampleTime,
addressGroup: addressGroup));
triggerTimeline.Add(new TrajectoryDoEvent(
waypointIndex: i,
triggerTime: triggerTime,
offsetCycles: program.OffsetValues[i],
holdCycles: holdCycles,
addressGroup: addressGroup));
}
return new ShotTimeline(shotEvents, triggerTimeline);
}
}

View File

@@ -0,0 +1,78 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Triggering;
/// <summary>
/// 把补中点后的规划轨迹映射回原始示教点的时间戳。
///
/// 为什么需要单独拆一层?
/// ---
/// self-adapt-icsp 会在原始示教点之间插入中点,导致 PlannedWaypoints 的数量和顺序
/// 与 OriginalProgram.Waypoints 不一致。但触发时序ShotEvent必须绑定到原始示教点
/// 而不是中点。这一层负责在补中点后的轨迹中“找回”每个原始示教点对应的精确时刻。
/// </summary>
public sealed class WaypointTimestampResolver
{
/// <summary>
/// 判定两个路点是否为同一示教点的关节空间距离容差。
/// </summary>
public const double DefaultPositionTolerance = 1e-4;
/// <summary>
/// 为原始示教点列表找回在规划轨迹中的对应时间戳。
/// </summary>
/// <param name="trajectory">规划后的轨迹(可能已补中点)。</param>
/// <returns>与原始示教点一一对应的时间戳数组(单位:秒)。</returns>
/// <exception cref="InvalidOperationException">当某个原始示教点无法在当前路点列表中匹配时抛出。</exception>
public double[] Resolve(PlannedTrajectory trajectory)
{
ArgumentNullException.ThrowIfNull(trajectory);
var originalWaypoints = trajectory.OriginalProgram.Waypoints;
var plannedWaypoints = trajectory.PlannedWaypoints;
var waypointTimes = trajectory.WaypointTimes;
var timestamps = new double[originalWaypoints.Count];
int plannedIndex = 0;
for (int originalIndex = 0; originalIndex < originalWaypoints.Count; originalIndex++)
{
bool matched = false;
for (; plannedIndex < plannedWaypoints.Count; plannedIndex++)
{
if (WaypointDistance(originalWaypoints[originalIndex], plannedWaypoints[plannedIndex]) <= DefaultPositionTolerance)
{
timestamps[originalIndex] = waypointTimes[plannedIndex];
matched = true;
plannedIndex++; // 下一次匹配从当前位置之后开始,保证顺序
break;
}
}
if (!matched)
{
throw new InvalidOperationException(
$"无法在补中点后的轨迹中为原始示教点 {originalIndex} 找回对应时间戳。");
}
}
return timestamps;
}
/// <summary>
/// 计算两个路点在关节空间的欧氏距离。
/// </summary>
private static double WaypointDistance(JointWaypoint a, JointWaypoint b)
{
double sumSq = 0.0;
int count = Math.Min(a.Positions.Count, b.Positions.Count);
for (int i = 0; i < count; i++)
{
double diff = a.Positions[i] - b.Positions[i];
sumSq += diff * diff;
}
return Math.Sqrt(sumSq);
}
}

View File

@@ -0,0 +1,9 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
</PropertyGroup>
</Project>

View File

@@ -0,0 +1,7 @@
<Project Sdk="Microsoft.NET.Sdk.Web">
<ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1,13 @@
var builder = WebApplication.CreateBuilder(args);
var app = builder.Build();
app.MapGet("/", () => Results.Redirect("/healthz"));
app.MapGet("/healthz", () => Results.Ok(new
{
status = "ok",
service = "flyshot-server-host"
}));
app.Run();
public partial class Program;

View File

@@ -0,0 +1,38 @@
{
"$schema": "http://json.schemastore.org/launchsettings.json",
"iisSettings": {
"windowsAuthentication": false,
"anonymousAuthentication": true,
"iisExpress": {
"applicationUrl": "http://localhost:43174",
"sslPort": 44344
}
},
"profiles": {
"http": {
"commandName": "Project",
"dotnetRunMessages": true,
"launchBrowser": true,
"applicationUrl": "http://localhost:5190",
"environmentVariables": {
"ASPNETCORE_ENVIRONMENT": "Development"
}
},
"https": {
"commandName": "Project",
"dotnetRunMessages": true,
"launchBrowser": true,
"applicationUrl": "https://localhost:7167;http://localhost:5190",
"environmentVariables": {
"ASPNETCORE_ENVIRONMENT": "Development"
}
},
"IIS Express": {
"commandName": "IISExpress",
"launchBrowser": true,
"environmentVariables": {
"ASPNETCORE_ENVIRONMENT": "Development"
}
}
}
}

View File

@@ -0,0 +1,8 @@
{
"Logging": {
"LogLevel": {
"Default": "Information",
"Microsoft.AspNetCore": "Warning"
}
}
}

View File

@@ -0,0 +1,9 @@
{
"Logging": {
"LogLevel": {
"Default": "Information",
"Microsoft.AspNetCore": "Warning"
}
},
"AllowedHosts": "*"
}

View File

@@ -0,0 +1,176 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Tests;
/// <summary>
/// 锁定 Task 3 的兼容输入行为,确保旧配置、.robot 元数据和路径策略都能被稳定加载。
/// </summary>
public sealed class ConfigCompatibilityTests
{
/// <summary>
/// 验证现有 RobotConfig.json 能被加载,并保持关键机器人参数与飞拍程序内容不变。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.True(loaded.Robot.UseDo);
Assert.Equal([7, 8], loaded.Robot.IoAddresses);
Assert.Equal(2, loaded.Robot.IoKeepCycles);
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
var program = Assert.Contains("001", loaded.Programs);
Assert.Equal("001", program.Name);
Assert.Equal(5, program.Waypoints.Count);
Assert.Equal(3, program.ShotWaypointCount);
Assert.Empty(program.AddressGroups[0].Addresses);
Assert.Equal([8, 7], program.AddressGroups[1].Addresses);
}
/// <summary>
/// 验证旧配置缺少 offset_values 和 addr 字段时,会自动回填与旧系统一致的默认值。
/// </summary>
[Fact]
public void RobotConfigLoader_FillsLegacyDefaults_WhenOptionalFieldsAreMissing()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": false,
"io_keep_cycles": 3,
"acc_limit": 0.5,
"jerk_limit": 0.25
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5]],
"shot_flags": [0, 1, 0]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
var program = Assert.Contains("demo", loaded.Programs);
Assert.False(loaded.Robot.UseDo);
Assert.Empty(loaded.Robot.IoAddresses);
Assert.Equal(3, loaded.Robot.IoKeepCycles);
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
Assert.Equal([0, 0, 0], program.OffsetValues);
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据,并构造规划侧可直接消费的 RobotProfile。
/// </summary>
[Fact]
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var profile = new RobotModelLoader().LoadProfile(modelPath);
Assert.Equal("FANUC_LR_Mate_200iD_7L", profile.Name);
Assert.Equal(modelPath, profile.ModelPath);
Assert.Equal(6, profile.DegreesOfFreedom);
Assert.Equal(6.45, profile.JointLimits[0].VelocityLimit, precision: 2);
Assert.Equal(29.81, profile.JointLimits[2].AccelerationLimit, precision: 2);
var coupling = Assert.Single(profile.JointCouplings);
Assert.Equal("Joint3", coupling.SlaveJointName);
Assert.Equal("Joint2", coupling.MasterJointName);
Assert.Equal(1.0, coupling.Multiplier);
Assert.Equal(0.0, coupling.Offset);
}
/// <summary>
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。
/// </summary>
[Fact]
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
}
/// <summary>
/// 验证路径兼容层既能补旧目录候选,也能按平台策略生成默认用户数据目录。
/// </summary>
[Fact]
public void PathCompatibility_ResolvesLegacyCandidates_AndBuildsUserDataRoots()
{
var tempRoot = CreateTempDirectory();
try
{
var legacyConfigPath = Path.Combine(tempRoot, "Rvbust", "Install", "FlyingShot", "Config", "sample.json");
Directory.CreateDirectory(Path.GetDirectoryName(legacyConfigPath)!);
File.WriteAllText(legacyConfigPath, "{}");
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
Assert.Equal(legacyConfigPath, resolved);
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 定位当前工作区根目录,便于复用父仓库中的真实样本。
/// </summary>
private static string GetWorkspaceRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
{
var slnPath = Path.Combine(current.FullName, "FlyshotReplacement.sln");
if (File.Exists(slnPath))
{
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
}
current = current.Parent;
}
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
/// <summary>
/// 创建当前测试专用的临时目录,避免不同测试之间相互污染。
/// </summary>
private static string CreateTempDirectory()
{
var tempPath = Path.Combine(Path.GetTempPath(), "flyshot-config-tests", Guid.NewGuid().ToString("N"));
Directory.CreateDirectory(tempPath);
return tempPath;
}
}

View File

@@ -0,0 +1,207 @@
using System.Text.Json;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Tests;
/// <summary>
/// Verifies the Task 2 domain contracts before planning and runtime code depend on them.
/// </summary>
public sealed class DomainModelTests
{
/// <summary>
/// Ensures robot profiles keep a stable copy of joint limits and reject invalid dimensions.
/// </summary>
[Fact]
public void RobotProfile_CopiesJointLimits_AndRejectsMismatchedDof()
{
var jointLimits = new[]
{
new JointLimit("J1", 7.85, 32.72, 272.7),
new JointLimit("J2", 6.63, 27.63, 230.28)
};
var profile = new RobotProfile(
name: "LR_Mate_200iD_7L",
modelPath: "Models/LR_Mate_200iD_7L.robot",
degreesOfFreedom: 2,
jointLimits: jointLimits,
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
Assert.Equal(2, profile.DegreesOfFreedom);
Assert.NotSame(jointLimits, profile.JointLimits);
// The planner must not accept a profile whose DOF and limits disagree.
Assert.Throws<ArgumentException>(() => new RobotProfile(
name: "InvalidRobot",
modelPath: "Models/Invalid.robot",
degreesOfFreedom: 3,
jointLimits: jointLimits,
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8)));
}
/// <summary>
/// Ensures uploaded flyshot programs keep their shot metadata aligned with teach waypoints.
/// </summary>
[Fact]
public void FlyshotProgram_RejectsMisalignedShotMetadata()
{
var waypoints = new[]
{
new JointWaypoint(new[] { 0.0, 1.0 }),
new JointWaypoint(new[] { 2.0, 3.0 })
};
var validProgram = new FlyshotProgram(
name: "EOL10_EAU_0",
waypoints: waypoints,
shotFlags: new[] { true, false },
offsetValues: new[] { 0, 1 },
addressGroups: new[]
{
new IoAddressGroup(new[] { 100 }),
new IoAddressGroup(Array.Empty<int>())
});
Assert.Equal(1, validProgram.ShotWaypointCount);
// The gateway cannot recover from count mismatches after this point, so fail fast here.
Assert.Throws<ArgumentException>(() => new FlyshotProgram(
name: "BrokenProgram",
waypoints: waypoints,
shotFlags: new[] { true },
offsetValues: new[] { 0, 1 },
addressGroups: new[]
{
new IoAddressGroup(new[] { 100 }),
new IoAddressGroup(Array.Empty<int>())
}));
}
/// <summary>
/// Ensures execution requests start from predictable defaults for compatibility paths.
/// </summary>
[Fact]
public void TrajectoryRequest_UsesExpectedDefaults()
{
var request = new TrajectoryRequest(
robot: CreateRobotProfile(),
program: CreateProgram(),
method: PlanningMethod.Icsp);
Assert.False(request.MoveToStart);
Assert.False(request.SaveTrajectoryArtifacts);
Assert.False(request.UseCache);
Assert.Equal(PlanningMethod.Icsp, request.Method);
}
/// <summary>
/// Ensures runtime snapshots expose safe empty collections before the controller connects.
/// </summary>
[Fact]
public void ControllerStateSnapshot_InitializesEmptyRuntimeCollections()
{
var snapshot = new ControllerStateSnapshot(
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
connectionState: "Disconnected",
isEnabled: false,
isInMotion: false,
speedRatio: 100.0);
Assert.Empty(snapshot.JointPositions);
Assert.Empty(snapshot.CartesianPose);
Assert.Empty(snapshot.ActiveAlarms);
}
/// <summary>
/// Ensures JSON payloads keep stable enum and property names for downstream SDKs.
/// </summary>
[Fact]
public void DomainObjects_SerializeStableContract()
{
var result = new TrajectoryResult(
programName: "EOL10_EAU_0",
method: PlanningMethod.SelfAdaptIcsp,
isValid: true,
duration: TimeSpan.FromSeconds(1.25),
shotEvents: new[]
{
new ShotEvent(
waypointIndex: 0,
triggerTime: 0.5,
sampleIndex: 62,
sampleTime: 0.496,
addressGroup: new IoAddressGroup(new[] { 100 }))
},
triggerTimeline: new[]
{
new TrajectoryDoEvent(
waypointIndex: 0,
triggerTime: 0.5,
offsetCycles: 0,
holdCycles: 1,
addressGroup: new IoAddressGroup(new[] { 100 }))
},
artifacts: new[]
{
new TrajectoryArtifact(
kind: TrajectoryArtifactKind.JointDenseTrajectory,
logicalName: "JointDetialTraj.txt",
relativePath: "artifacts/JointDetialTraj.txt")
},
failureReason: null,
usedCache: true,
originalWaypointCount: 4,
plannedWaypointCount: 5);
var json = JsonSerializer.Serialize(result);
Assert.Contains("\"programName\":\"EOL10_EAU_0\"", json);
Assert.Contains("\"method\":\"SelfAdaptIcsp\"", json);
Assert.Contains("\"kind\":\"JointDenseTrajectory\"", json);
Assert.Contains("\"usedCache\":true", json);
}
/// <summary>
/// Creates a representative robot profile for request-level domain tests.
/// </summary>
private static RobotProfile CreateRobotProfile()
{
return new RobotProfile(
name: "LR_Mate_200iD_7L",
modelPath: "Models/LR_Mate_200iD_7L.robot",
degreesOfFreedom: 2,
jointLimits: new[]
{
new JointLimit("J1", 7.85, 32.72, 272.7),
new JointLimit("J2", 6.63, 27.63, 230.28)
},
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
}
/// <summary>
/// Creates a representative uploaded program for request-level domain tests.
/// </summary>
private static FlyshotProgram CreateProgram()
{
return new FlyshotProgram(
name: "EOL10_EAU_0",
waypoints: new[]
{
new JointWaypoint(new[] { 0.0, 1.0 }),
new JointWaypoint(new[] { 2.0, 3.0 })
},
shotFlags: new[] { true, false },
offsetValues: new[] { 0, 1 },
addressGroups: new[]
{
new IoAddressGroup(new[] { 100 }),
new IoAddressGroup(Array.Empty<int>())
});
}
}

View File

@@ -0,0 +1,32 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.6.0" />
<PackageReference Include="xunit" Version="2.4.2" />
<PackageReference Include="xunit.runner.visualstudio" Version="2.4.5">
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
<PrivateAssets>all</PrivateAssets>
</PackageReference>
<PackageReference Include="coverlet.collector" Version="6.0.0">
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
<PrivateAssets>all</PrivateAssets>
</PackageReference>
</ItemGroup>
<ItemGroup>
<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.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1 @@
global using Xunit;

View File

@@ -0,0 +1,154 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Xunit.Abstractions;
namespace Flyshot.Core.Tests;
/// <summary>
/// 离线轨迹生成入口,与 Python demo 的 CLI 对齐。
///
/// 为什么放在测试项目里?
/// ---
/// 测试项目已经引用了所有需要的模块Config、Domain、Planning、Triggering
/// 不需要额外创建 Console 项目。通过 dotnet test --filter 可以直接调用,
/// 也能利用 xUnit 的断言做结果验证。
/// </summary>
public sealed class OfflinePlanTests
{
/// <summary>
/// 使用预设参数生成离线轨迹,输出到 analysis/output/dotnet/。
/// </summary>
[Theory]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)]
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)]
public void GenerateTrajectory_MatchesPythonDemo(
string configPath,
string robotModelPath,
string trajName,
bool useSelfAdapt,
double speedRatio)
{
var workspaceRoot = GetWorkspaceRoot();
var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}");
Directory.CreateDirectory(outputDir);
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(configPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath);
// 2. 应用 speed_ratio 缩放。
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);
// 3. 规划轨迹。
var request = new TrajectoryRequest(
robot: scaledProfile,
program: program,
method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.Icsp);
PlannedTrajectory trajectory;
if (useSelfAdapt)
{
trajectory = new SelfAdaptIcspPlanner().Plan(request, loadedConfig.Robot.AdaptIcspTryNum);
}
else
{
trajectory = new ICspPlanner().Plan(request);
}
// 4. 生成触发时间轴。
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
.Build(trajectory, holdCycles: loadedConfig.Robot.IoKeepCycles, samplePeriod: TimeSpan.FromMilliseconds(16));
// 5. 稠密采样。
double samplePeriod = 0.016;
var jointDense = TrajectorySampler.SampleJointTrajectory(trajectory, samplePeriod);
var cartDense = TrajectorySampler.SampleCartesianTrajectory(trajectory, kinematicsModel, samplePeriod);
// 6. 导出文件。
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.demo.txt"), jointDense);
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.demo.txt"), cartDense);
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.demo.json"), timeline.ShotEvents);
// 7. 打印统计信息到测试输出。
_testOutputHelper.WriteLine($"traj_name={trajName}");
_testOutputHelper.WriteLine($"speed_ratio={speedRatio:F6}");
_testOutputHelper.WriteLine($"duration={trajectory.WaypointTimes[^1]:F6}");
_testOutputHelper.WriteLine($"joint_dense_rows={jointDense.Count}");
_testOutputHelper.WriteLine($"cart_dense_rows={cartDense.Count}");
_testOutputHelper.WriteLine($"shot_events={timeline.ShotEvents.Count}");
_testOutputHelper.WriteLine($"output_dir={outputDir}");
// 最小验证:输出文件存在且非空。
Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.demo.txt")));
Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.demo.txt")));
Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.demo.json")));
Assert.True(jointDense.Count > 0);
Assert.True(cartDense.Count > 0);
}
private readonly ITestOutputHelper _testOutputHelper;
public OfflinePlanTests(ITestOutputHelper testOutputHelper)
{
_testOutputHelper = testOutputHelper;
}
/// <summary>
/// 按 speed_ratio 缩放机器人关节限制。
///
/// 缩放律:
/// - 速度按一次方speedRatio^1
/// - 加速度按平方speedRatio^2
/// - jerk 按立方speedRatio^3
///
/// 为什么加速度是平方?
/// ---
/// 如果把时间轴整体缩放 1/speedRatio 倍,速度按 speedRatio 缩放,加速度按 speedRatio^2 缩放。
/// 所以降低 speed_ratio 意味着降低速度、更大幅度地降低加速度和 jerk轨迹更平滑。
/// </summary>
private static RobotProfile ScaleRobotProfile(RobotProfile source, double speedRatio)
{
return new RobotProfile(
name: source.Name,
modelPath: source.ModelPath,
degreesOfFreedom: source.DegreesOfFreedom,
jointLimits: source.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
limit.VelocityLimit * speedRatio,
limit.AccelerationLimit * speedRatio * speedRatio,
limit.JerkLimit * speedRatio * speedRatio * speedRatio))
.ToArray(),
jointCouplings: source.JointCouplings,
servoPeriod: source.ServoPeriod,
triggerPeriod: source.TriggerPeriod);
}
/// <summary>
/// 定位父工作区根目录。
/// </summary>
private static string GetWorkspaceRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
{
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
{
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
}
current = current.Parent;
}
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
}

View File

@@ -0,0 +1,227 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Triggering;
namespace Flyshot.Core.Tests;
/// <summary>
/// 锁定 Task 4 的最小兼容面,覆盖 ICSP、自适应补点以及飞拍触发时间轴。
/// </summary>
public sealed class PlanningCompatibilityTests
{
/// <summary>
/// 验证 ICSP 规划至少会生成严格递增的 waypoint 时间轴。
/// </summary>
[Fact]
public void ICspPlanner_ReturnsMonotonicWaypointTimes()
{
var request = new TrajectoryRequest(
robot: CreateRobotProfile([1, 1, 1, 1, 1, 1], [2, 2, 2, 2, 2, 2], [10, 10, 10, 10, 10, 10]),
program: CreateProgram(
new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.4, 0.1, 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 }
}),
method: PlanningMethod.Icsp);
var trajectory = new ICspPlanner().Plan(request);
Assert.Equal(4, trajectory.WaypointTimes.Count);
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
}
/// <summary>
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
/// </summary>
[Fact]
public void SelfAdaptIcspPlanner_InsertsMidpoints_ForEol9Case()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
var constrainedProfile = ScaleRobotProfile(baseProfile, velocityScale: 0.9, accelerationScale: 0.9 * 0.9, jerkScale: 0.9 * 0.9 * 0.9);
var request = new TrajectoryRequest(
robot: constrainedProfile,
program: config.Programs["EOL9_EAU_90"],
method: PlanningMethod.SelfAdaptIcsp);
var trajectory = new SelfAdaptIcspPlanner().Plan(request, adaptIcspTryNum: config.Robot.AdaptIcspTryNum);
Assert.True(trajectory.InsertedWaypointCount > 0);
Assert.True(trajectory.PlannedWaypointCount > trajectory.OriginalWaypointCount);
Assert.True(trajectory.SegmentScales.Max() <= 1.0005);
}
/// <summary>
/// 验证补点后仍然能按原始示教点顺序找回时间戳,而不是错误地绑定到新增中点。
/// </summary>
[Fact]
public void WaypointTimestampResolver_UsesOriginalTeachPointsAfterInsertion()
{
var originalProgram = CreateProgram(
new[]
{
new[] { 0.0, 0.0 },
new[] { 1.0, 0.0 },
new[] { 2.0, 0.0 },
new[] { 3.0, 0.0 }
});
var trajectory = new PlannedTrajectory(
robot: CreateRobotProfile([1, 1], [1, 1], [1, 1]),
originalProgram: originalProgram,
plannedWaypoints:
[
new JointWaypoint([0.0, 0.0]),
new JointWaypoint([0.5, 0.0]),
new JointWaypoint([1.0, 0.0]),
new JointWaypoint([2.0, 0.0]),
new JointWaypoint([2.5, 0.0]),
new JointWaypoint([3.0, 0.0])
],
waypointTimes: [0.0, 0.25, 0.5, 1.0, 1.5, 2.0],
segmentDurations: [0.25, 0.25, 0.5, 0.5, 0.5],
segmentScales: [1.0, 1.0, 1.0, 1.0, 1.0],
method: PlanningMethod.SelfAdaptIcsp,
iterations: 3,
threshold: 0.0);
var timestamps = new WaypointTimestampResolver().Resolve(trajectory);
Assert.Equal([0.0, 0.5, 1.0, 2.0], timestamps);
}
/// <summary>
/// 验证触发时间轴会使用原始 waypoint 时间、offset 周期和地址组生成 ShotEvent/TrajectoryDoEvent。
/// </summary>
[Fact]
public void ShotTimelineBuilder_MapsOffsetsToShotEventsAndTriggerTimeline()
{
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
var program = new FlyshotProgram(
name: "demo",
waypoints:
[
new JointWaypoint([0.0, 0.0]),
new JointWaypoint([1.0, 0.0]),
new JointWaypoint([2.0, 0.0])
],
shotFlags: [false, true, false],
offsetValues: [0, 1, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup([2, 4]),
new IoAddressGroup(Array.Empty<int>())
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.5, 1.0],
segmentDurations: [0.5, 0.5],
segmentScales: [1.0, 1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
.Build(trajectory, holdCycles: 2, samplePeriod: TimeSpan.FromMilliseconds(16));
var shotEvent = Assert.Single(timeline.ShotEvents);
Assert.Equal(0.508, shotEvent.TriggerTime, precision: 3);
Assert.Equal([2, 4], shotEvent.AddressGroup.Addresses);
var doEvent = Assert.Single(timeline.TriggerTimeline);
Assert.Equal(1, doEvent.WaypointIndex);
Assert.Equal(1, doEvent.OffsetCycles);
Assert.Equal(2, doEvent.HoldCycles);
}
/// <summary>
/// 构造一个最小 RobotProfile便于规划层单元测试聚焦在时间轴逻辑上。
/// </summary>
private static RobotProfile CreateRobotProfile(
IReadOnlyList<double> velocityLimits,
IReadOnlyList<double> accelerationLimits,
IReadOnlyList<double> jerkLimits)
{
return new RobotProfile(
name: "TestRobot",
modelPath: "Models/Test.robot",
degreesOfFreedom: velocityLimits.Count,
jointLimits: velocityLimits
.Select((velocity, index) => new JointLimit(
$"J{index + 1}",
velocity,
accelerationLimits[index],
jerkLimits[index]))
.ToArray(),
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
}
/// <summary>
/// 构造一个默认不带拍照点的最小 FlyshotProgram。
/// </summary>
private static FlyshotProgram CreateProgram(IEnumerable<double[]> waypointRows)
{
var rows = waypointRows.ToArray();
return new FlyshotProgram(
name: "demo",
waypoints: rows.Select(row => new JointWaypoint(row)).ToArray(),
shotFlags: Enumerable.Repeat(false, rows.Length).ToArray(),
offsetValues: Enumerable.Repeat(0, rows.Length).ToArray(),
addressGroups: Enumerable.Range(0, rows.Length).Select(_ => new IoAddressGroup(Array.Empty<int>())).ToArray());
}
/// <summary>
/// 在不丢失 couple 元数据的前提下,按比例缩放机器人关节限制。
/// </summary>
private static RobotProfile ScaleRobotProfile(RobotProfile source, double velocityScale, double accelerationScale, double jerkScale)
{
return new RobotProfile(
name: source.Name,
modelPath: source.ModelPath,
degreesOfFreedom: source.DegreesOfFreedom,
jointLimits: source.JointLimits
.Select(limit => new JointLimit(
limit.JointName,
limit.VelocityLimit * velocityScale,
limit.AccelerationLimit * accelerationScale,
limit.JerkLimit * jerkScale))
.ToArray(),
jointCouplings: source.JointCouplings,
servoPeriod: source.ServoPeriod,
triggerPeriod: source.TriggerPeriod);
}
/// <summary>
/// 定位父工作区根目录,读取真实的 EOL9 样本和机器人模型。
/// </summary>
private static string GetWorkspaceRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
{
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
{
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
}
current = current.Parent;
}
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
}

View File

@@ -0,0 +1,30 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="Microsoft.AspNetCore.Mvc.Testing" Version="8.0.0" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.6.0" />
<PackageReference Include="xunit" Version="2.4.2" />
<PackageReference Include="xunit.runner.visualstudio" Version="2.4.5">
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
<PrivateAssets>all</PrivateAssets>
</PackageReference>
<PackageReference Include="coverlet.collector" Version="6.0.0">
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
<PrivateAssets>all</PrivateAssets>
</PackageReference>
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\..\src\Flyshot.Server.Host\Flyshot.Server.Host.csproj" />
</ItemGroup>
</Project>

View File

@@ -0,0 +1 @@
global using Xunit;

View File

@@ -0,0 +1,27 @@
using System.Net;
using System.Text.Json;
using Microsoft.AspNetCore.Mvc.Testing;
namespace Flyshot.Server.IntegrationTests;
public sealed class FlyshotServerFactory : WebApplicationFactory<Program>;
public sealed class HealthEndpointTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
{
[Fact]
public async Task GetHealthz_ReturnsOkPayload()
{
using var client = factory.CreateClient();
using var response = await client.GetAsync("/healthz");
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
await using var responseStream = await response.Content.ReadAsStreamAsync();
using var jsonDocument = await JsonDocument.ParseAsync(responseStream);
var root = jsonDocument.RootElement;
Assert.Equal("ok", root.GetProperty("status").GetString());
Assert.Equal("flyshot-server-host", root.GetProperty("service").GetString());
}
}