✨ 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:
398
.gitignore
vendored
Normal file
398
.gitignore
vendored
Normal 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
150
AGENTS.md
Normal 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
150
CLAUDE.md
Normal 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
8
Directory.Build.props
Normal 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
146
FlyshotReplacement.sln
Normal 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
24
README.md
Normal 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 状态页
|
||||
13
src/Flyshot.Core.Config/Flyshot.Core.Config.csproj
Normal file
13
src/Flyshot.Core.Config/Flyshot.Core.Config.csproj
Normal 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>
|
||||
137
src/Flyshot.Core.Config/PathCompatibility.cs
Normal file
137
src/Flyshot.Core.Config/PathCompatibility.cs
Normal 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));
|
||||
}
|
||||
}
|
||||
278
src/Flyshot.Core.Config/RobotConfigLoader.cs
Normal file
278
src/Flyshot.Core.Config/RobotConfigLoader.cs
Normal 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();
|
||||
}
|
||||
}
|
||||
216
src/Flyshot.Core.Config/RobotModelLoader.cs
Normal file
216
src/Flyshot.Core.Config/RobotModelLoader.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
95
src/Flyshot.Core.Domain/ControllerStateSnapshot.cs
Normal file
95
src/Flyshot.Core.Domain/ControllerStateSnapshot.cs
Normal 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; }
|
||||
}
|
||||
9
src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj
Normal file
9
src/Flyshot.Core.Domain/Flyshot.Core.Domain.csproj
Normal file
@@ -0,0 +1,9 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
</Project>
|
||||
175
src/Flyshot.Core.Domain/FlyshotProgram.cs
Normal file
175
src/Flyshot.Core.Domain/FlyshotProgram.cs
Normal 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; }
|
||||
}
|
||||
172
src/Flyshot.Core.Domain/RobotKinematicsModel.cs
Normal file
172
src/Flyshot.Core.Domain/RobotKinematicsModel.cs
Normal 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; }
|
||||
}
|
||||
226
src/Flyshot.Core.Domain/RobotProfile.cs
Normal file
226
src/Flyshot.Core.Domain/RobotProfile.cs
Normal 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; }
|
||||
}
|
||||
88
src/Flyshot.Core.Domain/RuntimeAlarm.cs
Normal file
88
src/Flyshot.Core.Domain/RuntimeAlarm.cs
Normal 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
|
||||
}
|
||||
71
src/Flyshot.Core.Domain/ShotEvent.cs
Normal file
71
src/Flyshot.Core.Domain/ShotEvent.cs
Normal 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; }
|
||||
}
|
||||
86
src/Flyshot.Core.Domain/TrajectoryRequest.cs
Normal file
86
src/Flyshot.Core.Domain/TrajectoryRequest.cs
Normal 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
|
||||
}
|
||||
272
src/Flyshot.Core.Domain/TrajectoryResult.cs
Normal file
272
src/Flyshot.Core.Domain/TrajectoryResult.cs
Normal 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
|
||||
}
|
||||
298
src/Flyshot.Core.Planning/CubicSplineInterpolator.cs
Normal file
298
src/Flyshot.Core.Planning/CubicSplineInterpolator.cs
Normal 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 + d,t 相对于段起点归一化到 [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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
66
src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs
Normal file
66
src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs
Normal 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));
|
||||
}
|
||||
}
|
||||
13
src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj
Normal file
13
src/Flyshot.Core.Planning/Flyshot.Core.Planning.csproj
Normal 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>
|
||||
190
src/Flyshot.Core.Planning/ICspPlanner.cs
Normal file
190
src/Flyshot.Core.Planning/ICspPlanner.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
83
src/Flyshot.Core.Planning/Kinematics/Mat4.cs
Normal file
83
src/Flyshot.Core.Planning/Kinematics/Mat4.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
214
src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs
Normal file
214
src/Flyshot.Core.Planning/Kinematics/RobotKinematics.cs
Normal 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 },
|
||||
};
|
||||
}
|
||||
}
|
||||
130
src/Flyshot.Core.Planning/PlannedTrajectory.cs
Normal file
130
src/Flyshot.Core.Planning/PlannedTrajectory.cs
Normal 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; }
|
||||
}
|
||||
106
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs
Normal file
106
src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs
Normal file
@@ -0,0 +1,106 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning.Kinematics;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 从规划轨迹生成稠密关节/笛卡尔采样序列。
|
||||
///
|
||||
/// 为什么采样周期默认 0.016s?
|
||||
/// ---
|
||||
/// 真机伺服周期是 8ms(125Hz),但规划层通常以 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;
|
||||
}
|
||||
}
|
||||
182
src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs
Normal file
182
src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs
Normal 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());
|
||||
}
|
||||
}
|
||||
14
src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj
Normal file
14
src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj
Normal 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>
|
||||
31
src/Flyshot.Core.Triggering/ShotTimeline.cs
Normal file
31
src/Flyshot.Core.Triggering/ShotTimeline.cs
Normal 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; }
|
||||
}
|
||||
82
src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs
Normal file
82
src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
78
src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs
Normal file
78
src/Flyshot.Core.Triggering/WaypointTimestampResolver.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
9
src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj
Normal file
9
src/Flyshot.Runtime.Common/Flyshot.Runtime.Common.csproj
Normal file
@@ -0,0 +1,9 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
</Project>
|
||||
7
src/Flyshot.Server.Host/Flyshot.Server.Host.csproj
Normal file
7
src/Flyshot.Server.Host/Flyshot.Server.Host.csproj
Normal 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>
|
||||
13
src/Flyshot.Server.Host/Program.cs
Normal file
13
src/Flyshot.Server.Host/Program.cs
Normal 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;
|
||||
38
src/Flyshot.Server.Host/Properties/launchSettings.json
Normal file
38
src/Flyshot.Server.Host/Properties/launchSettings.json
Normal 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"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
8
src/Flyshot.Server.Host/appsettings.Development.json
Normal file
8
src/Flyshot.Server.Host/appsettings.Development.json
Normal file
@@ -0,0 +1,8 @@
|
||||
{
|
||||
"Logging": {
|
||||
"LogLevel": {
|
||||
"Default": "Information",
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
}
|
||||
}
|
||||
9
src/Flyshot.Server.Host/appsettings.json
Normal file
9
src/Flyshot.Server.Host/appsettings.json
Normal file
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"Logging": {
|
||||
"LogLevel": {
|
||||
"Default": "Information",
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
},
|
||||
"AllowedHosts": "*"
|
||||
}
|
||||
176
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
Normal file
176
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
207
tests/Flyshot.Core.Tests/DomainModelTests.cs
Normal file
207
tests/Flyshot.Core.Tests/DomainModelTests.cs
Normal 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>())
|
||||
});
|
||||
}
|
||||
}
|
||||
32
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
Normal file
32
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
Normal 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>
|
||||
1
tests/Flyshot.Core.Tests/GlobalUsings.cs
Normal file
1
tests/Flyshot.Core.Tests/GlobalUsings.cs
Normal file
@@ -0,0 +1 @@
|
||||
global using Xunit;
|
||||
154
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
Normal file
154
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
Normal 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.");
|
||||
}
|
||||
}
|
||||
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal file
227
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
Normal 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.");
|
||||
}
|
||||
}
|
||||
@@ -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>
|
||||
1
tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs
Normal file
1
tests/Flyshot.Server.IntegrationTests/GlobalUsings.cs
Normal file
@@ -0,0 +1 @@
|
||||
global using Xunit;
|
||||
27
tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs
Normal file
27
tests/Flyshot.Server.IntegrationTests/HealthEndpointTests.cs
Normal 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());
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user