Compare commits
5 Commits
4eeaa3fef3
...
0292e077ff
| Author | SHA1 | Date | |
|---|---|---|---|
| 0292e077ff | |||
| 69fa3edd89 | |||
| 390d066ece | |||
| a78e6761cb | |||
| 8a20d9f507 |
14
.claude/settings.local.json
Normal file
14
.claude/settings.local.json
Normal file
@@ -0,0 +1,14 @@
|
||||
{
|
||||
"permissions": {
|
||||
"allow": [
|
||||
"Bash(git commit -m ':*)",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln -v minimal 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test FlyshotReplacement.sln --no-build -v minimal 2>&1')",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal)",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj --no-build -v minimal)",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json'\\)\\); json.load\\(open\\('.vscode/tasks.json'\\)\\); print\\('JSON valid.'\\)\")",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")"
|
||||
]
|
||||
}
|
||||
}
|
||||
67
.vscode/launch.json
vendored
Normal file
67
.vscode/launch.json
vendored
Normal file
@@ -0,0 +1,67 @@
|
||||
{
|
||||
// VS Code 启动与调试配置
|
||||
// 依赖 C# 扩展（OmniSharp 或 C# Dev Kit）提供 coreclr 调试器。
|
||||
// 文档：https://code.visualstudio.com/docs/csharp/debugger-settings
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
{
|
||||
// 标准调试启动：编译并启动 Host，命中断点，浏览器自动打开首页
|
||||
"name": ".NET Core Launch (Host)",
|
||||
"type": "coreclr",
|
||||
"request": "launch",
|
||||
"program": "dotnet",
|
||||
"args": [
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--no-launch-profile"
|
||||
],
|
||||
"cwd": "${workspaceFolder}",
|
||||
"env": {
|
||||
"ASPNETCORE_ENVIRONMENT": "Development",
|
||||
"ASPNETCORE_URLS": "http://localhost:5190"
|
||||
},
|
||||
"stopAtEntry": false,
|
||||
"console": "internalConsole",
|
||||
"preLaunchTask": "build",
|
||||
"serverReadyAction": {
|
||||
"action": "openExternally",
|
||||
"pattern": "\\bNow listening on:\\s+(https?://\\S+)",
|
||||
"uriFormat": "%s"
|
||||
}
|
||||
},
|
||||
{
|
||||
// 热重载调试启动：自动编译、自动重启、断点保留；迭代 Web / 控制器层时首选
|
||||
"name": ".NET Core Watch (Host)",
|
||||
"type": "coreclr",
|
||||
"request": "launch",
|
||||
"program": "dotnet",
|
||||
"args": [
|
||||
"watch",
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--no-launch-profile"
|
||||
],
|
||||
"cwd": "${workspaceFolder}",
|
||||
"env": {
|
||||
"ASPNETCORE_ENVIRONMENT": "Development",
|
||||
"ASPNETCORE_URLS": "http://localhost:5190"
|
||||
},
|
||||
"stopAtEntry": false,
|
||||
"console": "integratedTerminal",
|
||||
"serverReadyAction": {
|
||||
"action": "openExternally",
|
||||
"pattern": "\\bNow listening on:\\s+(https?://\\S+)",
|
||||
"uriFormat": "%s"
|
||||
}
|
||||
},
|
||||
{
|
||||
// 附加到正在运行的 dotnet 进程（如已手动 `dotnet run` 或 Windows Service 模式）
|
||||
"name": ".NET Core Attach",
|
||||
"type": "coreclr",
|
||||
"request": "attach",
|
||||
"processId": "${command:pickProcess}"
|
||||
}
|
||||
]
|
||||
}
|
||||
160
.vscode/tasks.json
vendored
Normal file
160
.vscode/tasks.json
vendored
Normal file
@@ -0,0 +1,160 @@
|
||||
{
|
||||
// VS Code 任务配置
|
||||
// 文档：https://code.visualstudio.com/docs/editor/tasks
|
||||
"version": "2.0.0",
|
||||
"tasks": [
|
||||
{
|
||||
// 构建整个解决方案，是 launch.json 启动前的默认 preLaunchTask
|
||||
"label": "build",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"build",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln",
|
||||
"/property:GenerateFullPaths=true",
|
||||
"/consoleloggerparameters:NoSummary",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": {
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
},
|
||||
"problemMatcher": "$msCompile",
|
||||
"presentation": {
|
||||
"reveal": "silent",
|
||||
"clear": true
|
||||
}
|
||||
},
|
||||
{
|
||||
// 仅构建宿主项目，迭代 Web 层时比整解决方案快
|
||||
"label": "build-host",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"build",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"/property:GenerateFullPaths=true",
|
||||
"/consoleloggerparameters:NoSummary",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "build",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 还原 NuGet 包，新增引用或克隆后第一次打开时使用
|
||||
"label": "restore",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"restore",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln"
|
||||
],
|
||||
"problemMatcher": []
|
||||
},
|
||||
{
|
||||
// 清理所有项目的 bin/obj
|
||||
"label": "clean",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"clean",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln"
|
||||
],
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 跑全部测试（领域 + 集成）
|
||||
"label": "test",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/FlyshotReplacement.sln",
|
||||
"--no-restore",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": {
|
||||
"kind": "test",
|
||||
"isDefault": true
|
||||
},
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 仅跑领域 / 算法层测试，迭代规划逻辑时使用
|
||||
"label": "test-core",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "test",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 仅跑宿主集成测试，迭代 HTTP / 控制器层时使用
|
||||
"label": "test-integration",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"test",
|
||||
"${workspaceFolder}/tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj",
|
||||
"-v",
|
||||
"minimal"
|
||||
],
|
||||
"group": "test",
|
||||
"problemMatcher": "$msCompile"
|
||||
},
|
||||
{
|
||||
// 启动宿主，供 launch.json 的 watch 配置作为前置任务
|
||||
"label": "watch",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"watch",
|
||||
"run",
|
||||
"--project",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"--launch-profile",
|
||||
"http"
|
||||
],
|
||||
"isBackground": true,
|
||||
"problemMatcher": {
|
||||
"owner": "dotnet-watch",
|
||||
"pattern": [
|
||||
{
|
||||
"regexp": "^.*$",
|
||||
"file": 1,
|
||||
"location": 2,
|
||||
"message": 3
|
||||
}
|
||||
],
|
||||
"background": {
|
||||
"activeOnStart": true,
|
||||
"beginsPattern": "^.*Watch run started.*$",
|
||||
"endsPattern": "^.*Application started.*$"
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
// Release 配置发布到 publish/，用于现场部署包打包
|
||||
"label": "publish",
|
||||
"command": "dotnet",
|
||||
"type": "process",
|
||||
"args": [
|
||||
"publish",
|
||||
"${workspaceFolder}/src/Flyshot.Server.Host/Flyshot.Server.Host.csproj",
|
||||
"-c",
|
||||
"Release",
|
||||
"-o",
|
||||
"${workspaceFolder}/publish"
|
||||
],
|
||||
"problemMatcher": "$msCompile"
|
||||
}
|
||||
]
|
||||
}
|
||||
36
AGENTS.md
36
AGENTS.md
@@ -8,7 +8,7 @@
|
||||
|
||||
- 使用 `C# + .NET 8`
|
||||
- 提供跨平台独立服务端
|
||||
- 兼容现有 `50001/TCP+JSON` 上层接入语义
|
||||
- 以新的 ASP.NET Core HTTP API 作为唯一上层接口
|
||||
- 重写轨迹生成、触发时序、FANUC 控制链路和状态监控
|
||||
- Windows / Linux 都能运行完整服务端
|
||||
- 只支持当前现场这套组合
|
||||
@@ -18,6 +18,7 @@
|
||||
- GUI 桌面程序
|
||||
- 多机器人同时控制
|
||||
- 面向多控制柜的通用平台化框架
|
||||
- 恢复旧 `50001/TCP+JSON` 网关
|
||||
|
||||
## 2. 代码与资料边界
|
||||
|
||||
@@ -37,7 +38,12 @@
|
||||
flyshot-replacement/
|
||||
├─ src/
|
||||
│ ├─ Flyshot.Server.Host/
|
||||
│ ├─ Flyshot.ControllerClientCompat/
|
||||
│ ├─ Flyshot.Core.Config/
|
||||
│ ├─ Flyshot.Core.Domain/
|
||||
│ ├─ Flyshot.Core.Planning/
|
||||
│ ├─ Flyshot.Core.Triggering/
|
||||
│ ├─ Flyshot.Runtime.Fanuc/
|
||||
│ └─ Flyshot.Runtime.Common/
|
||||
├─ tests/
|
||||
│ ├─ Flyshot.Server.IntegrationTests/
|
||||
@@ -64,8 +70,10 @@ flyshot-replacement/
|
||||
- `Flyshot.Core.Triggering`
|
||||
- `TrajectoryDO` 等价时间轴
|
||||
- `shot_flags / offset_values / addr` 解析
|
||||
- `Flyshot.LegacyGateway`
|
||||
- `50001/TCP+JSON` 兼容接入
|
||||
- `Flyshot.ControllerClientCompat`
|
||||
- HTTP 控制器后端兼容服务
|
||||
- 旧 `ControllerClient` 语义适配
|
||||
- 不启动 `50001/TCP+JSON` 监听
|
||||
- `Flyshot.Runtime.Fanuc`
|
||||
- `10010 / 10012 / 60015`
|
||||
- `Flyshot.Web.Status`
|
||||
@@ -84,6 +92,7 @@ flyshot-replacement/
|
||||
|
||||
### 4.2 实现约束
|
||||
|
||||
- 旧 `ControllerClient` 资料只作为接口语义参考；运行时入口以新 HTTP API 为准，不恢复旧 `50001/TCP+JSON` 网关。
|
||||
- 旧协议兼容以“语义兼容”为主，不追求二进制逐字节一致。
|
||||
- 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。
|
||||
- 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。
|
||||
@@ -135,6 +144,20 @@ flyshot-replacement/
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
### 6.1 父目录资料引用约定
|
||||
|
||||
- 日常开发、测试和 Codex 会话默认从 `flyshot-replacement/` 根目录启动。
|
||||
- 当前仓库内的 `@` 引用默认只覆盖本仓库文件，不要假设它能索引父目录资料。
|
||||
- 引用父目录资料时，统一直接写明确路径，优先使用相对路径，例如：
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/ControllerServer_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
- 当路径较长或跨工具复制时，可以使用绝对路径，但在文档和注释中优先保留相对路径写法，便于仓库整体搬迁。
|
||||
- 父目录中的 `analysis/`、`FlyingShot/`、`RobotController/`、`RPS/` 默认视为参考资料区，不在这些目录中继续落地新实现。
|
||||
- 新实现、测试、兼容层代码、设计文档和运行说明，都应优先写入 `flyshot-replacement/` 内部。
|
||||
- 如果父目录资料中的某段结论会长期影响本仓库实现，应在本仓库 `docs/` 中补充归纳说明，并标明来源路径，而不是要求后续开发反复回看聊天记录。
|
||||
- 如果需要引用父目录样本文件做测试输入，优先通过只读方式加载；只有在测试需要固化样本且样本已明确收敛时，才复制到本仓库测试数据目录。
|
||||
|
||||
## 7. 任务推进方式
|
||||
|
||||
- `README.md` 中的 Todo 需要随着阶段推进同步更新。
|
||||
@@ -148,3 +171,10 @@ flyshot-replacement/
|
||||
- `Flyshot.Server.Host` 已提供最小 `/healthz`。
|
||||
- 最小集成测试已通过。
|
||||
- 解决方案构建已通过。
|
||||
- 新 HTTP API / HTTP-only `ControllerClientCompat` 已覆盖旧 HTTP 控制器后端的主要兼容语义。
|
||||
- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp`，并已完成旧系统导出轨迹对齐。
|
||||
- `Flyshot.Core.Triggering` 已能从 `shot_flags / offset_values / addr` 生成触发时间轴。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路，但 J519 闭环与现场联调仍需补齐。
|
||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
||||
|
||||
@@ -64,8 +64,10 @@ flyshot-replacement/
|
||||
- `Flyshot.Core.Triggering`
|
||||
- `TrajectoryDO` 等价时间轴
|
||||
- `shot_flags / offset_values / addr` 解析
|
||||
- `Flyshot.LegacyGateway`
|
||||
- `50001/TCP+JSON` 兼容接入
|
||||
- `Flyshot.ControllerClientCompat`
|
||||
- HTTP 控制器后端兼容服务
|
||||
- 旧 `ControllerClient` 语义适配
|
||||
- 不启动 `50001/TCP+JSON` 监听
|
||||
- `Flyshot.Runtime.Fanuc`
|
||||
- `10010 / 10012 / 60015`
|
||||
- `Flyshot.Web.Status`
|
||||
|
||||
@@ -23,6 +23,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Planning", "sr
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Core.Triggering", "src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj", "{E4DDC34C-9AB6-4050-A927-3DF69804708A}"
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.ControllerClientCompat", "src\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj", "{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}"
|
||||
EndProject
|
||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Flyshot.Runtime.Fanuc", "src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj", "{B705FA6C-19CA-44A8-882C-6CE26A5379C9}"
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|Any CPU = Debug|Any CPU
|
||||
@@ -129,6 +133,30 @@ Global
|
||||
{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
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x64.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x64.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x86.ActiveCfg = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Debug|x86.Build.0 = Debug|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x64.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x64.Build.0 = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x86.ActiveCfg = Release|Any CPU
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3}.Release|x86.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|Any CPU.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x64.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x64.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x86.ActiveCfg = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Debug|x86.Build.0 = Debug|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|Any CPU.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|Any CPU.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x64.Build.0 = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.ActiveCfg = Release|Any CPU
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9}.Release|x86.Build.0 = Release|Any CPU
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
@@ -142,5 +170,7 @@ Global
|
||||
{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}
|
||||
{5B45CC23-3551-4D0F-B3CC-22659C2A8BA3} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
|
||||
{B705FA6C-19CA-44A8-882C-6CE26A5379C9} = {64EABE09-B1E0-4476-A213-32C93E46E7C3}
|
||||
EndGlobalSection
|
||||
EndGlobal
|
||||
|
||||
69
README.md
69
README.md
@@ -4,7 +4,7 @@
|
||||
|
||||
当前目标：
|
||||
|
||||
- 兼容现有 `50001/TCP+JSON` 上层接入语义
|
||||
- 以新的 ASP.NET Core HTTP API 作为唯一上层接口
|
||||
- 重写轨迹生成、触发时序和 FANUC 实时控制链路
|
||||
- 提供 Web 状态监控页面
|
||||
- 在 Windows 和 Linux 上运行完整后台服务
|
||||
@@ -13,12 +13,75 @@
|
||||
|
||||
- 这是长期运行的无头后台服务，不是 GUI 桌面程序。
|
||||
- 第一版仅面向当前现场组合，后续再扩展机型与控制柜适配。
|
||||
- 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口；旧 `ControllerClient` 逆向资料只作为接口语义参考，不作为运行时目标。
|
||||
- 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
|
||||
- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。
|
||||
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 真机抓包确认为 90B 固定帧。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送能力，但 J519 闭环和现场联调仍需补齐。
|
||||
|
||||
当前 Todo：
|
||||
开发约定：
|
||||
|
||||
- 建议从 `flyshot-replacement/` 根目录启动 IDE、终端和 Codex 会话。
|
||||
- 当前仓库内的 `@` 引用主要覆盖本仓库文件；引用父目录资料时，请直接写相对路径，如 `../analysis/ICSP_algorithm_reverse_analysis.md`。
|
||||
- 父目录中的 `analysis/`、`FlyingShot/`、`RobotController/`、`RPS/` 主要作为逆向参考资料和样本来源，新实现默认只落地在当前仓库。
|
||||
|
||||
当前已完成：
|
||||
|
||||
- [x] 初始化独立仓库
|
||||
- [x] 创建 `dotnet 8` 解决方案骨架
|
||||
- [x] 打通最小宿主与 `/healthz`
|
||||
- [x] 建立领域模型与模块边界
|
||||
- [x] 落地配置兼容与机器人模型解析
|
||||
- [ ] 落地轨迹规划、实时控制和 Web 状态页
|
||||
- [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴
|
||||
- [x] 完成 ICSP 轨迹导出结果与旧系统对齐
|
||||
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路
|
||||
- [x] 落地 Web 状态页
|
||||
- [x] 落地浏览器内 OpenAPI 自动驱动的接口调试页（`/debug`），与 `Swagger:Enabled` 同步可见
|
||||
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码，确认 `10010` 状态帧为 90B
|
||||
- [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发
|
||||
- [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析
|
||||
- [x] 保留新 HTTP 接口路线，明确不再实现旧 `50001/TCP+JSON` 网关
|
||||
|
||||
剩余 Todo：
|
||||
|
||||
1. 配置与测试基线
|
||||
- [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移：`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`，应改用稳定样本或更新断言。
|
||||
- [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。
|
||||
- [ ] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流（`/debug` 页已提供交互式覆盖，仍需补静态文档说明现场调用顺序）。
|
||||
|
||||
2. 轨迹规划
|
||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
||||
- [ ] 补齐 `save_traj` / `SaveTrajInfo` 的规划结果导出，将稠密关节轨迹、笛卡尔轨迹和 ShotEvents 写入可诊断 artifacts。
|
||||
|
||||
3. FANUC TCP 10012 命令通道
|
||||
- [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。
|
||||
- [x] 所有命令响应必须检查 `result_code`，失败时返回可诊断错误，而不是只更新本地缓存。
|
||||
|
||||
4. FANUC TCP 10010 状态通道
|
||||
- [x] 用 `j519 协议.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||
- [x] 补充断线清理和异常帧拒绝测试。
|
||||
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
||||
|
||||
5. FANUC UDP 60015 J519 运动链路
|
||||
- [ ] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界，避免双重节拍或命令覆盖。
|
||||
- [ ] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位闭环检查。
|
||||
- [ ] 校验序号递增、响应滞后、丢包、停止包和最后一帧语义。
|
||||
- [ ] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐。
|
||||
|
||||
6. 真机联调与运行安全
|
||||
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
||||
- [ ] 增加急停、伺服未就绪、程序未启动、网络断开、控制柜拒收命令等故障路径处理。
|
||||
- [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。
|
||||
- [ ] 增加运行日志、协议摘要日志和状态快照导出，便于现场排查。
|
||||
|
||||
7. 发布与部署
|
||||
- [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。
|
||||
- [ ] 补充生产配置模板、端口说明和现场部署检查表。
|
||||
- [ ] 给 Web 状态页增加真机连接、程序状态、J519 状态位和最近报警显示。
|
||||
|
||||
11
TrajectoryStore/FANUC_LR_Mate_200iD_trajectories.json
Normal file
11
TrajectoryStore/FANUC_LR_Mate_200iD_trajectories.json
Normal file
@@ -0,0 +1,11 @@
|
||||
{
|
||||
"robot": {
|
||||
"use_do": false,
|
||||
"io_addr": [],
|
||||
"io_keep_cycles": 2,
|
||||
"acc_limit": 1,
|
||||
"jerk_limit": 1,
|
||||
"adapt_icsp_try_num": 5
|
||||
},
|
||||
"flying_shots": {}
|
||||
}
|
||||
140
docs/controller-client-api-compatibility-requirements.md
Normal file
140
docs/controller-client-api-compatibility-requirements.md
Normal file
@@ -0,0 +1,140 @@
|
||||
# ControllerClient API 兼容逆向约束
|
||||
|
||||
> 记录时间：2026-04-24
|
||||
> 适用仓库：`flyshot-replacement`
|
||||
> 当前阶段：已落地 HTTP-only `ControllerClientCompat` 服务，并已将轨迹执行接入规划、触发时间轴和最小运行时骨架；不实现 `50001/TCP+JSON` 监听
|
||||
|
||||
## 1. 当前目标
|
||||
|
||||
本轮目标不是直接实现 `50001/TCP+JSON` 兼容网关，而是先把旧 `ControllerClient` 暴露的公开 API 做成可执行的逆向合同。
|
||||
|
||||
本轮交付物固定为两份文档：
|
||||
|
||||
- `docs/controller-client-api-compatibility-requirements.md`
|
||||
- `docs/controller-client-api-reverse-engineering.md`
|
||||
|
||||
后续继续扩展 `Flyshot.ControllerClientCompat` 的方法覆盖、兼容测试矩阵或真实控制器联动时，必须以这两份文档为准，不再重新口头约定接口语义。
|
||||
|
||||
## 2. 范围边界
|
||||
|
||||
本轮只覆盖 `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` 中的 32 个公开方法。
|
||||
|
||||
分组如下：
|
||||
|
||||
- 传输与版本：`ConnectServer`、`GetServerVersion`、`GetClientVersion`
|
||||
- 机器人初始化：`SetUpRobot`、`SetUpRobotFromEnv`、`IsSetUp`、`SetShowTCP`、`GetName`、`GetDoF`
|
||||
- 控制器状态：`SetActiveController`、`Connect`、`Disconnect`、`EnableRobot`、`DisableRobot`、`StopMove`
|
||||
- 参数与 IO：`GetSpeedRatio`、`SetSpeedRatio`、`GetTCP`、`SetTCP`、`GetIO`、`SetIO`
|
||||
- 运动与求解：`GetJointPosition`、`GetPose`、`GetNearestIK`、`MoveJoint`、`ExecuteTrajectory`
|
||||
- 飞拍轨迹：`UploadFlyShotTraj`、`DeleteFlyShotTraj`、`ListFlyShotTraj`、`ExecuteFlyShotTraj`、`SaveTrajInfo`、`IsFlyShotTrajValid`
|
||||
|
||||
明确不在本轮范围内：
|
||||
|
||||
- `ControllerServer` 内部所有未公开 `_Xxx` 方法的完整实现复原
|
||||
- `50001` 网关代码、TCP server、JSON parser、命令路由实现
|
||||
- 真机 `10010 / 10012 / 60015` 联调
|
||||
- 抓包、hook、反汇编级协议完全坐实
|
||||
- 把当前 replacement 仓库的实现约束误写成旧系统事实
|
||||
|
||||
## 3. 证据源优先级
|
||||
|
||||
逆向结论必须按以下优先级交叉确认：
|
||||
|
||||
1. `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
2. `../FlyingShot/FlyingShot/Example/UseControllerClient.cpp`
|
||||
3. `../FlyingShot/FlyingShot/Example/UseControllerClient.py`
|
||||
4. `../FlyingShot/FlyingShot/Example/UseRealRobot.py`
|
||||
5. `../FlyingShot/FlyingShot/Docs/用户手册/FANUC飞拍软件及SDK用户手册.md`
|
||||
6. `../analysis/ControllerServer_analysis.md`
|
||||
7. `../analysis/CommonMsg_protocol_analysis.md`
|
||||
8. `../analysis/Trajectory_generation_algorithm_analysis.md`
|
||||
9. `../FlyingShot/FlyingShot/Lib/libControllerClient.so` 与 `../FlyingShot/FlyingShot/Python/ControllerServer/ControllerServer.cpython-37m-x86_64-linux-gnu.so` 的字符串证据
|
||||
|
||||
使用规则：
|
||||
|
||||
- 头文件负责定义“公开合同”：方法名、参数名、默认值、返回类型。
|
||||
- 示例和手册负责定义“典型调用方式”：调用顺序、Python 包装形态、用户侧常见用法。
|
||||
- 逆向分析文档和二进制字符串负责定义“服务端映射”和“协议线索”：`_Xxx` 方法名、错误文本、命令名、部分字段顺序。
|
||||
- 不能从当前 replacement 仓库的未来设计反推旧系统事实。
|
||||
|
||||
## 4. 文档填写规则
|
||||
|
||||
主归档文档 `docs/controller-client-api-reverse-engineering.md` 对每个 API 必须固定填写以下项目：
|
||||
|
||||
- C++ 公开签名
|
||||
- Python 包装形态
|
||||
- 服务端归属
|
||||
- 协议 / 命令线索
|
||||
- 返回值与默认值
|
||||
- 典型工作流位置
|
||||
- 证据来源
|
||||
- 置信度
|
||||
- 待确认点
|
||||
|
||||
填写约束：
|
||||
|
||||
- 只要精确 JSON 包结构还未恢复，就明确写成“命令名已知，JSON envelope 未恢复”。
|
||||
- 只要 Python 失败路径没有样例，就不能假装已经确认失败时的返回形态。
|
||||
- `GetServerVersion` 这类没有显式 `_GetServerVersion` 的接口，必须标成“协议分发层行为”，不能伪造一个服务端实现名。
|
||||
- `ConnectServer` 与 `GetClientVersion` 必须明确标成客户端侧行为，不写进服务端命令表。
|
||||
- 对 `GetJointPosition -> _GetJointPositions` 这种命名不一致，要单独注明。
|
||||
- 当同一结论同时来自头文件、示例和字符串时，置信度可标为“高”；只有示例间接体现时，最多“中”。
|
||||
|
||||
## 5. 验收条件
|
||||
|
||||
本轮逆向归档完成时，至少满足：
|
||||
|
||||
- 32 个公开方法全部覆盖，且每个方法只出现一次
|
||||
- 29 个服务端相关 API 有明确映射或明确写成协议分发层行为
|
||||
- `ConnectServer`、`GetClientVersion` 两个客户端侧行为被明确排除在服务端命令表外
|
||||
- 四条工作流附录完整：
|
||||
- 初始化工作流
|
||||
- 控制器状态工作流
|
||||
- 普通轨迹工作流
|
||||
- 飞拍轨迹工作流
|
||||
- 已知高置信协议字段至少记录：
|
||||
- `GetSpeedRatio`
|
||||
- `SetSpeedRatio`
|
||||
- `GetIO`
|
||||
- `SetIO`
|
||||
- 仍未恢复的部分必须进入“待确认问题”清单，不能被静默略过
|
||||
|
||||
## 6. 当前已确认摘要
|
||||
|
||||
当前已确认的高价值结论如下：
|
||||
|
||||
- `ControllerClient.h` 中共有 32 个公开方法。
|
||||
- 其中 29 个方法可与 `ControllerServer` 的公开 `_Xxx` 方法一一对齐。
|
||||
- `GetServerVersion` 能看到明确字符串证据，但未恢复到显式 `_GetServerVersion` 实现，更接近 `_ClientCB` / `_IsJsonValid` 所在的协议分发层。
|
||||
- `ConnectServer` 是客户端建立到 `127.0.0.1:50001` 的传输层动作。
|
||||
- `GetClientVersion` 更像客户端库自身版本查询，不进入服务端命令表。
|
||||
- `GetSpeedRatio` / `SetSpeedRatio`、`GetIO` / `SetIO` 已有较高置信度的底层字段顺序与 `MsgID` 证据。
|
||||
- `UploadFlyShotTraj`、`ListFlyShotTraj` 存在额外字符串线索：
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
- `GetNextListFlyShotTraj`
|
||||
- `ExecuteFlyShotTraj`、`IsFlyShotTrajValid`、`SaveTrajInfo` 在工作流上有清晰分工，不能混成一个“执行轨迹”接口。
|
||||
|
||||
## 7. 下轮实现约束
|
||||
|
||||
当前 HTTP-only 兼容层已经可以承接公开 API 的主要服务端语义，并且 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已经进入 replacement 自身的规划与运行时链路。后续扩展必须遵守：
|
||||
|
||||
- 先以逆向合同建命令表，再写 `TCP + JSON` 入口
|
||||
- 先做兼容测试矩阵，再补最小命令桩
|
||||
- 区分“旧系统事实”和“replacement 当前策略”
|
||||
- 真机未接通前，允许实现层返回稳定错误或模拟状态，但不能反过来污染逆向文档
|
||||
|
||||
## 8. 当前 replacement 实现状态
|
||||
|
||||
以下内容是当前新实现的状态，不反推为旧系统事实：
|
||||
|
||||
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。
|
||||
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
||||
- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
|
||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试。
|
||||
- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态；真实 `10010 / 10012 / 60015` Socket 通讯与现场联调尚未落地。
|
||||
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
561
docs/controller-client-api-reverse-engineering.md
Normal file
561
docs/controller-client-api-reverse-engineering.md
Normal file
@@ -0,0 +1,561 @@
|
||||
# ControllerClient API 全量逆向归档
|
||||
|
||||
> 记录时间：2026-04-24
|
||||
> 适用仓库：`flyshot-replacement`
|
||||
> 目标：为后续 `50001/TCP+JSON` 兼容网关实现提供只读合同
|
||||
|
||||
## 1. 总览
|
||||
|
||||
本次归档覆盖 `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h` 中全部 32 个公开方法。
|
||||
|
||||
当前分类结果：
|
||||
|
||||
- 29 个 API 可与 `ControllerServer` 公开 `_Xxx` 方法一一映射
|
||||
- 1 个 API 属于协议分发层行为：`GetServerVersion`
|
||||
- 2 个 API 属于客户端侧行为：`ConnectServer`、`GetClientVersion`
|
||||
|
||||
本文档中的“协议 / 命令线索”有两个层次：
|
||||
|
||||
- 命令名：已从头文件、字符串或逆向文档坐实，例如 `SetUpRobot`、`GetIO`
|
||||
- JSON envelope：请求 JSON 的精确键名、必填规则、整体结构；当前大多仍未完全恢复
|
||||
|
||||
因此，除 `GetSpeedRatio` / `SetSpeedRatio` / `GetIO` / `SetIO` 等已在命令通道层坐实字段顺序的接口外，其余接口默认只写“命令名已知，精确 JSON 包结构待确认”。
|
||||
|
||||
置信度定义：
|
||||
|
||||
- 高：头文件 + 示例/手册 + 字符串/逆向文档三方交叉确认
|
||||
- 中：头文件 + 示例，或头文件 + 字符串，缺少第三方交叉
|
||||
- 低：仅有间接线索，未达到本轮归档主结论标准
|
||||
|
||||
## 2. 传输与版本
|
||||
|
||||
### `ConnectServer`
|
||||
|
||||
- C++ 公开签名：`bool ConnectServer(const std::string &server_ip = "127.0.0.1", unsigned port = 50001);`
|
||||
- Python 包装形态：`c.ConnectServer(server_ip="127.0.0.1", port=50001) -> bool`
|
||||
- 服务端归属：客户端传输层行为，不进入 `ControllerServer._Xxx` 命令表
|
||||
- 协议 / 命令线索：建立到 `127.0.0.1:50001` 的 TCP 连接；JSON 负载只发生在连接建立之后
|
||||
- 返回值与默认值：默认 `server_ip="127.0.0.1"`、`port=50001`；成功返回 `true`
|
||||
- 典型工作流位置：所有远程 API 的前置步骤
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`
|
||||
- 置信度：高
|
||||
- 待确认点：重连、超时、握手细节未恢复
|
||||
|
||||
### `GetServerVersion`
|
||||
|
||||
- C++ 公开签名：`bool GetServerVersion(std::string &version);`
|
||||
- Python 包装形态：样例中表现为 `controller.GetServerVersion() -> str`
|
||||
- 服务端归属：协议分发层行为；当前未恢复到显式 `_GetServerVersion`
|
||||
- 协议 / 命令线索：命令名字符串 `GetServerVersion` 已坐实；服务端存在 `GetServerVersion success: {}.` 日志；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：C++ 为 `bool + out string`；Python 样例把结果当作直接字符串使用
|
||||
- 典型工作流位置：`ConnectServer` 之后、机器人初始化前后均可读取的元信息接口
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`libControllerClient.so`、`ControllerServer.cpython-37m-x86_64-linux-gnu.so`
|
||||
- 置信度：高
|
||||
- 待确认点：Python 失败路径的返回形态未看到样例；请求 JSON 的精确字段未恢复
|
||||
|
||||
### `GetClientVersion`
|
||||
|
||||
- C++ 公开签名：`bool GetClientVersion(std::string &version);`
|
||||
- Python 包装形态：未见公开样例；从包装库字符串看接口存在
|
||||
- 服务端归属：客户端本地行为，不进入服务端命令表
|
||||
- 协议 / 命令线索：`libControllerClient.so` 与 `PyControllerClient` 都有 `GetClientVersion` 符号；未见 `ControllerServer` 对应 `_Xxx`
|
||||
- 返回值与默认值：C++ 为 `bool + out string`；Python 侧返回形态待确认
|
||||
- 典型工作流位置：客户端自检或 SDK 版本上报，不依赖服务端状态
|
||||
- 证据来源：`ControllerClient.h`、`libControllerClient.so`、`PyControllerClient.cpython-37m-x86_64-linux-gnu.so`
|
||||
- 置信度：高
|
||||
- 待确认点：实际版本字符串来源与 Python 包装失败行为未恢复
|
||||
|
||||
## 3. 机器人初始化
|
||||
|
||||
### `SetUpRobot`
|
||||
|
||||
- C++ 公开签名：`bool SetUpRobot(const std::string &robot_name);`
|
||||
- Python 包装形态：`c.SetUpRobot("FANUC_LR_Mate_200iD") -> bool`
|
||||
- 服务端归属：`ControllerServer._SetUpRobot`
|
||||
- 协议 / 命令线索：命令名 `SetUpRobot` 已坐实；参数高概率是 `robot_name`；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：创建客户端并连上 `50001` 后首先调用；旧说明明确它是最先执行的服务端初始化动作
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：请求字段键名是否为 `robot_name` 仍需抓包或 hook 坐实
|
||||
|
||||
### `SetUpRobotFromEnv`
|
||||
|
||||
- C++ 公开签名：`bool SetUpRobotFromEnv(const std::string &env_file);`
|
||||
- Python 包装形态：未见公开样例；服务端与客户端字符串均存在
|
||||
- 服务端归属：`ControllerServer._SetUpRobotFromEnv`
|
||||
- 协议 / 命令线索：命令名 `SetUpRobotFromEnv` 已坐实；参数高概率是环境文件绝对路径；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：与 `SetUpRobot` 二选一；当现场通过环境文件而不是型号名初始化时使用
|
||||
- 证据来源：`ControllerClient.h`、`ControllerServer_analysis.md`、`ControllerServer` 字符串、`libControllerClient.so`
|
||||
- 置信度：高
|
||||
- 待确认点：环境文件路径是否必须为绝对路径由注释可见，但服务端是否做额外归一化未恢复
|
||||
|
||||
### `IsSetUp`
|
||||
|
||||
- C++ 公开签名：`bool IsSetUp();`
|
||||
- Python 包装形态：`c.IsSetUp() -> bool`
|
||||
- 服务端归属：`ControllerServer._IsSetUp`
|
||||
- 协议 / 命令线索：命令名 `IsSetUp` 已坐实；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：直接返回布尔值
|
||||
- 典型工作流位置：`SetUpRobot` 或 `SetUpRobotFromEnv` 之后的状态确认
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：服务端返回是否仅受机器人对象存在性控制，还是还依赖模型/控制器更深层状态，当前未完全恢复
|
||||
|
||||
### `SetShowTCP`
|
||||
|
||||
- C++ 公开签名：`bool SetShowTCP(bool is_show = true, double axis_length = 0.1, size_t axis_size = 2);`
|
||||
- Python 包装形态：`c.SetShowTCP(is_show=True, axis_length=0.1, axis_size=2) -> bool`
|
||||
- 服务端归属：`ControllerServer._SetShowTCP`
|
||||
- 协议 / 命令线索：命令名 `SetShowTCP` 已坐实；字符串中可见 `SetShowTCP is_show success/failed`；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：默认 `is_show=true`、`axis_length=0.1`、`axis_size=2`
|
||||
- 典型工作流位置：初始化完成后、切换控制器前的仿真显示配置
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`ControllerServer` 字符串
|
||||
- 置信度：高
|
||||
- 待确认点：仅仿真控制器生效还是真实控制器也接受该命令，当前未见明确负例
|
||||
|
||||
### `GetName`
|
||||
|
||||
- C++ 公开签名：`std::string GetName();`
|
||||
- Python 包装形态：`c.GetName() -> str`
|
||||
- 服务端归属：`ControllerServer._GetName`
|
||||
- 协议 / 命令线索：命令名 `GetName` 已坐实；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：直接返回机器人名称字符串
|
||||
- 典型工作流位置：完成机器人初始化后读取名称确认
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：失败时是返回空串还是异常/默认值，Python 样例未覆盖
|
||||
|
||||
### `GetDoF`
|
||||
|
||||
- C++ 公开签名：`int GetDoF();`
|
||||
- Python 包装形态：`c.GetDoF() -> int`
|
||||
- 服务端归属：`ControllerServer._GetDoF`
|
||||
- 协议 / 命令线索：命令名 `GetDoF` 已坐实；精确 JSON envelope 未恢复
|
||||
- 返回值与默认值：直接返回自由度整数
|
||||
- 典型工作流位置：完成机器人初始化后读取自由度
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：失败时的客户端行为未恢复
|
||||
|
||||
## 4. 控制器状态
|
||||
|
||||
### `SetActiveController`
|
||||
|
||||
- C++ 公开签名：`bool SetActiveController(bool sim = true);`
|
||||
- Python 包装形态：`c.SetActiveController(sim=True) -> bool`；真机样例使用 `sim=False`
|
||||
- 服务端归属：`ControllerServer._SetActiveController`
|
||||
- 协议 / 命令线索：命令名 `SetActiveController` 已坐实；字符串中可见 `SetActiveController sim success/failed`
|
||||
- 返回值与默认值：默认 `sim=true`
|
||||
- 典型工作流位置：机器人初始化后、`Connect` 前，用于在仿真控制器和真实控制器之间切换
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：切换控制器时是否会隐式断开旧控制器，当前只有 `_DisconnectAll` 的间接线索
|
||||
|
||||
### `Connect`
|
||||
|
||||
- C++ 公开签名：`bool Connect(const std::string &robot_ip);`
|
||||
- Python 包装形态：`c.Connect("192.168.10.101") -> bool`
|
||||
- 服务端归属：`ControllerServer._Connect`
|
||||
- 协议 / 命令线索：命令名 `Connect` 已坐实；字符串中可见 `Connect ip success/failed`
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：选定活动控制器后，通知服务端连接真实机器人 IP
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 键名是否为 `ip` 或 `robot_ip` 未恢复；仿真模式下是否接受同一命令仍需实现侧验证
|
||||
|
||||
### `Disconnect`
|
||||
|
||||
- C++ 公开签名：`bool Disconnect();`
|
||||
- Python 包装形态：`c.Disconnect() -> bool`
|
||||
- 服务端归属：`ControllerServer._Disconnect`
|
||||
- 协议 / 命令线索：命令名 `Disconnect` 已坐实；字符串中可见 `Disconnect success/failed`
|
||||
- 返回值与默认值：直接返回布尔值
|
||||
- 典型工作流位置：真实控制器断开或工作流结束时调用
|
||||
- 证据来源：`ControllerClient.h`、`ControllerServer_analysis.md`、`ControllerServer` 字符串、`libControllerClient.so`
|
||||
- 置信度：高
|
||||
- 待确认点：是否同步清理状态通道 / 命令通道 / 伺服通道，当前只在 FANUC 分析文档看到对象链，并未完全坐实释放顺序
|
||||
|
||||
### `EnableRobot`
|
||||
|
||||
- C++ 公开签名：`bool EnableRobot(unsigned buffer_size = 2);`
|
||||
- Python 包装形态：`c.EnableRobot() -> bool`
|
||||
- 服务端归属：`ControllerServer._EnableRobot`
|
||||
- 协议 / 命令线索：命令名 `EnableRobot` 已坐实；字符串中可见 `EnableRobot success/failed`
|
||||
- 返回值与默认值：默认 `buffer_size=2`
|
||||
- 典型工作流位置：`Connect` 后使能机器人；多数运动 API 前置条件
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：`buffer_size` 在旧服务端具体如何映射到伺服缓冲区参数，当前未完全恢复
|
||||
|
||||
### `DisableRobot`
|
||||
|
||||
- C++ 公开签名：`bool DisableRobot();`
|
||||
- Python 包装形态：`c.DisableRobot() -> bool`
|
||||
- 服务端归属：`ControllerServer._DisableRobot`
|
||||
- 协议 / 命令线索：命令名 `DisableRobot` 已坐实；字符串中可见 `DisableRobot success/failed`
|
||||
- 返回值与默认值：直接返回布尔值
|
||||
- 典型工作流位置：停机或退出前关闭机器人使能
|
||||
- 证据来源：`ControllerClient.h`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：与 `StopMove` 的先后顺序约束未恢复
|
||||
|
||||
### `StopMove`
|
||||
|
||||
- C++ 公开签名：`bool StopMove();`
|
||||
- Python 包装形态：`c.StopMove() -> bool`
|
||||
- 服务端归属：`ControllerServer._StopMove`
|
||||
- 协议 / 命令线索：命令名 `StopMove` 已坐实；字符串中可见 `StopMove success/failed`
|
||||
- 返回值与默认值：直接返回布尔值
|
||||
- 典型工作流位置：当前运动中止；Python 示例中调用后会再次 `EnableRobot()`
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：是否会清空控制器执行队列、是否必须重新使能，当前只从示例看到倾向性而非硬约束
|
||||
|
||||
## 5. 参数与 IO
|
||||
|
||||
### `GetSpeedRatio`
|
||||
|
||||
- C++ 公开签名：`double GetSpeedRatio();`
|
||||
- Python 包装形态：`c.GetSpeedRatio() -> float`
|
||||
- 服务端归属：`ControllerServer._GetSpeedRatio`
|
||||
- 协议 / 命令线索：命令通道 `MsgID = 0x2206`；响应字段为 `ratio_int` 与 `result_code`；成功后客户端将 `ratio_int / 100.0`
|
||||
- 返回值与默认值：直接返回 `0~1` 之间的倍率
|
||||
- 典型工作流位置：连机并使能后读取当前速度倍率
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`
|
||||
- 置信度：高
|
||||
- 待确认点：服务端 JSON 层到命令通道层的参数桥接结构未恢复
|
||||
|
||||
### `SetSpeedRatio`
|
||||
|
||||
- C++ 公开签名：`bool SetSpeedRatio(double ratio);`
|
||||
- Python 包装形态：`c.SetSpeedRatio(0.8) -> bool`
|
||||
- 服务端归属：`ControllerServer._SetSpeedRatio`
|
||||
- 协议 / 命令线索：命令通道 `MsgID = 0x2207`；请求字段为 `ratio_int_0_100`；输入 `double` 先乘 `100` 并夹到 `[0,100]`
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：连机并使能后修改控制器速度倍率
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：ratio 越界时客户端是裁剪还是直接失败，当前更偏向裁剪，但缺少公开负例
|
||||
|
||||
### `GetTCP`
|
||||
|
||||
- C++ 公开签名：`bool GetTCP(Pose &tcp);`
|
||||
- Python 包装形态：`res, tcp = c.GetTCP()`
|
||||
- 服务端归属：`ControllerServer._GetTCP`
|
||||
- 协议 / 命令线索：控制器命令通道底层接口 `GetTCP` 已有 `MsgID = 0x2200` 证据，但 `ControllerClient` JSON 层请求结构未完全恢复
|
||||
- 返回值与默认值：C++ 为 `bool + out Pose`；Python 为 `(bool, Pose)`
|
||||
- 典型工作流位置：连机后读取当前控制器 TCP
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 层是否支持多个 TCP ID，目前只看到底层命令通道有 `tcp_id`
|
||||
|
||||
### `SetTCP`
|
||||
|
||||
- C++ 公开签名：`bool SetTCP(const Pose &tcp);`
|
||||
- Python 包装形态：`c.SetTCP(tcp) -> bool`
|
||||
- 服务端归属：`ControllerServer._SetTCP`
|
||||
- 协议 / 命令线索：控制器命令通道底层接口 `SetTCP` 已有 `MsgID = 0x2201` 证据；JSON 层精确字段未恢复
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：连机后修改控制器 TCP
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 层是否也暴露 `tcp_id` 概念尚未恢复
|
||||
|
||||
### `GetIO`
|
||||
|
||||
- C++ 公开签名：`bool GetIO(unsigned port, bool &value, IOType type = kIOTypeDI);`
|
||||
- Python 包装形态：`res, value = c.GetIO(port=1, io_type=IOType.kIOTypeDI)`
|
||||
- 服务端归属：`ControllerServer._GetIO`
|
||||
- 协议 / 命令线索：命令通道 `MsgID = 0x2208`；请求字段顺序是 `io_type`、`io_index`；响应字段是 `result_code`、`io_value`
|
||||
- 返回值与默认值：默认 `type = kIOTypeDI`；C++ 为 `bool + out bool`；Python 为 `(bool, bool)`
|
||||
- 典型工作流位置：连机后读取 DI/DO/RI/RO 等 IO 值
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：高层 JSON 是否把字段命名为 `port` / `value` / `type`，还是 `io_index` / `io_type`，当前未抓到完整包
|
||||
|
||||
### `SetIO`
|
||||
|
||||
- C++ 公开签名：`bool SetIO(unsigned port, bool value, IOType type = kIOTypeDO);`
|
||||
- Python 包装形态：`c.SetIO(port=1, value=True, io_type=IOType.kIOTypeDO) -> bool`
|
||||
- 服务端归属：`ControllerServer._SetIO`
|
||||
- 协议 / 命令线索：命令通道 `MsgID = 0x2209`；请求字段顺序是 `io_type`、`io_index`、`io_value`
|
||||
- 返回值与默认值：默认 `type = kIOTypeDO`
|
||||
- 典型工作流位置：连机后设置数字输出；飞拍链路之外的普通 IO 调试也会用到
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`CommonMsg_protocol_analysis.md`、`FANUC_realtime_comm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：布尔值在高层 JSON 中是否以 `true/false` 传输、在命令通道中以 `float` 传输，当前只坐实了命令通道层
|
||||
|
||||
## 6. 运动与求解
|
||||
|
||||
### `GetJointPosition`
|
||||
|
||||
- C++ 公开签名：`bool GetJointPosition(JointPositions &joint_position);`
|
||||
- Python 包装形态：`res, joints = c.GetJointPosition()`
|
||||
- 服务端归属：`ControllerServer._GetJointPositions`
|
||||
- 协议 / 命令线索：命令名 `GetJointPositions` 已坐实；客户端公开 API 名与服务端方法名存在单复数差异
|
||||
- 返回值与默认值：C++ 为 `bool + out JointPositions`；Python 为 `(bool, JointPositions)`
|
||||
- 典型工作流位置：读取当前关节角，常作为 `GetNearestIK` 的 seed 或 `MoveJoint` 的基准
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 层返回数组字段键名未恢复
|
||||
|
||||
### `GetPose`
|
||||
|
||||
- C++ 公开签名：`bool GetPose(Pose &pose);`
|
||||
- Python 包装形态：`res, pose = c.GetPose()`
|
||||
- 服务端归属：`ControllerServer._GetPose`
|
||||
- 协议 / 命令线索：命令名 `GetPose` 已坐实；字符串中可见 `GetPose success/failed`
|
||||
- 返回值与默认值：C++ 为 `bool + out Pose`；Python 为 `(bool, Pose)`
|
||||
- 典型工作流位置：读取当前 TCP 位姿，常与 `GetNearestIK` 配套使用
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：位姿坐标系定义虽然在 `Types.h` 中体现为 7 元数组，但 JSON 层字段结构未恢复
|
||||
|
||||
### `GetNearestIK`
|
||||
|
||||
- C++ 公开签名：`bool GetNearestIK(const Pose &pose, const JointPositions &seed, JointPositions &ik);`
|
||||
- Python 包装形态：`res, ik = c.GetNearestIK(pose, joint_seed=joints)`
|
||||
- 服务端归属：`ControllerServer._GetNearestIK`
|
||||
- 协议 / 命令线索：命令名 `GetNearestIK` 已坐实；字符串中可见 `GetNearestIK success/failed`
|
||||
- 返回值与默认值：C++ 为 `bool + out JointPositions`；Python 为 `(bool, JointPositions)`
|
||||
- 典型工作流位置：先 `GetPose` 得到当前位姿，再构造目标位姿，并使用当前关节或邻近关节作为 seed 求最近 IK
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 层中 seed 参数键名在 Python 里表现为 `joint_seed`，但 C++ 注释写的是 `seed`；高层字段命名仍待确认
|
||||
|
||||
### `MoveJoint`
|
||||
|
||||
- C++ 公开签名：`bool MoveJoint(const JointPositions &joint_position);`
|
||||
- Python 包装形态：`c.MoveJoint(home_joint) -> bool`
|
||||
- 服务端归属：`ControllerServer._MoveJoint`
|
||||
- 协议 / 命令线索：命令名 `MoveJoint` 已坐实；字符串中可见 `MoveJoint waypoint success/failed`
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：点到点回零或移动到飞拍轨迹起点
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`ControllerServer_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：移动速度/平滑参数是否固定在服务端内部，公开 API 未暴露
|
||||
|
||||
### `ExecuteTrajectory`
|
||||
|
||||
- C++ 公开签名：`bool ExecuteTrajectory(const std::vector<JointPositions> &waypoints, const std::string &method = "icsp", bool save_traj = false);`
|
||||
- Python 包装形态：`c.ExecuteTrajectory(waypoints=[...], method="icsp", save_traj=True) -> bool`
|
||||
- 服务端归属：`ControllerServer._ExecuteTrajectory`
|
||||
- 协议 / 命令线索：命令名 `ExecuteTrajectory` 已坐实；`method` 至少支持 `icsp` 与 `doubles`
|
||||
- 返回值与默认值：默认 `method="icsp"`、`save_traj=false`
|
||||
- 典型工作流位置：普通多点轨迹执行，不带飞拍 IO；输入是关节空间稀疏 waypoint，而不是笛卡尔点
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 中 waypoint 列表的键名和序列化结构未恢复；`doubles` 的失败语义未在公开样例中体现
|
||||
|
||||
## 7. 飞拍轨迹
|
||||
|
||||
### `UploadFlyShotTraj`
|
||||
|
||||
- C++ 公开签名：`bool UploadFlyShotTraj(const std::string &name, const std::vector<JointPositions> &waypoints, const std::vector<bool> &shot_flags, const std::vector<int> &offset_values, const std::vector<std::vector<int>> &addrs);`
|
||||
- Python 包装形态：`c.UploadFlyShotTraj(name="test_traj", waypoints=..., shot_flags=..., offset_values=..., addrs=...) -> bool`
|
||||
- 服务端归属：`ControllerServer._UploadFlyShotTraj`
|
||||
- 协议 / 命令线索：命令名 `UploadFlyShotTraj` 已坐实；客户端字符串中可见 `StartUploadFlyShotTraj` 与 `EndUploadFlyShotTraj`，说明上传阶段很可能分为开始 / 传输 / 结束三个子步骤
|
||||
- 返回值与默认值：成功返回 `true`；无默认参数
|
||||
- 典型工作流位置：运行时动态构造飞拍轨迹时，先把轨迹定义登记到服务端
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、`libControllerClient.so`、`ControllerServer` 字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 上传是否一次性发送完整 payload，还是像字符串暗示的那样分片上传，当前未完全恢复
|
||||
|
||||
### `DeleteFlyShotTraj`
|
||||
|
||||
- C++ 公开签名：`bool DeleteFlyShotTraj(const std::string &name);`
|
||||
- Python 包装形态：`c.DeleteFlyShotTraj(name="test_traj") -> bool`
|
||||
- 服务端归属：`ControllerServer._DeleteFlyShotTraj`
|
||||
- 协议 / 命令线索：命令名 `DeleteFlyShotTraj` 已坐实；字符串中可见 `DeleteFlyShotTraj {} success/failed`
|
||||
- 返回值与默认值：成功返回 `true`
|
||||
- 典型工作流位置：删除已上传的临时飞拍轨迹定义
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、SDK 手册、`ControllerServer_analysis.md`、`PyControllerClient` 字符串、`ControllerServer` 字符串
|
||||
- 置信度：高
|
||||
- 待确认点：删除配置内固有轨迹名与删除运行时上传轨迹时是否走同一条路径，当前未恢复
|
||||
|
||||
### `ListFlyShotTraj`
|
||||
|
||||
- C++ 公开签名：`std::vector<std::string> ListFlyShotTraj();`
|
||||
- Python 包装形态：`c.ListFlyShotTraj() -> list[str]`
|
||||
- 服务端归属：`ControllerServer._ListFlyShotTraj`
|
||||
- 协议 / 命令线索：命令名 `ListFlyShotTraj` 已坐实；客户端与服务端字符串都出现 `GetNextListFlyShotTraj`，说明底层列举很可能是迭代式获取，而不是一次性返回整个数组
|
||||
- 返回值与默认值：直接返回轨迹名称列表
|
||||
- 典型工作流位置：查看当前服务端已登记的飞拍轨迹
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py`、`ControllerServer_analysis.md`、`libControllerClient.so`、`ControllerServer` 字符串
|
||||
- 置信度：高
|
||||
- 待确认点：高层 JSON 是否真的直接返回数组，还是客户端内部循环拉取后再拼成数组，当前未完全恢复
|
||||
|
||||
### `ExecuteFlyShotTraj`
|
||||
|
||||
- C++ 公开签名：`bool ExecuteFlyShotTraj(const std::string &name, const bool move_to_start = false, const std::string &method = "icsp", bool save_traj = false, bool use_cache = false);`
|
||||
- Python 包装形态：`c.ExecuteFlyShotTraj(name="002", move_to_start=True, method="icsp", save_traj=True, use_cache=False) -> bool`
|
||||
- 服务端归属：`ControllerServer._ExecuteFlyShotTraj`
|
||||
- 协议 / 命令线索：命令名 `ExecuteFlyShotTraj` 已坐实；字符串中出现 `move_to_start`、`use_cache`；伪代码级逆向已恢复其主流程
|
||||
- 返回值与默认值：默认 `move_to_start=false`、`method="icsp"`、`save_traj=false`、`use_cache=false`
|
||||
- 典型工作流位置：对已存在的飞拍轨迹定义执行“生成 + 挂接 DO 时间轴 + 执行”
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.cpp`、`UseControllerClient.py`、`UseRealRobot.py`、SDK 手册、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：JSON 包结构未恢复；`use_cache` 的缓存键是否只按 `name`，当前从伪代码判断是，但未见更底层证据
|
||||
|
||||
### `SaveTrajInfo`
|
||||
|
||||
- C++ 公开签名：`bool SaveTrajInfo(const std::string &name, const std::string &method = "icsp");`
|
||||
- Python 包装形态：未见运行样例，但包装库和服务端都有明确字符串；语义可由手册和分析文档确认
|
||||
- 服务端归属：`ControllerServer._SaveTrajInfo`
|
||||
- 协议 / 命令线索：命令名 `SaveTrajInfo` 已坐实；字符串中可见 `SaveTrajInfo {} success/failed`
|
||||
- 返回值与默认值：默认 `method="icsp"`；`self-adapt-icsp` 在执行时保存分析文件会回落成 `icsp` 导出语义
|
||||
- 典型工作流位置：按给定 `name + method` 生成并导出轨迹分析文件，例如 `JointTraj.txt`、`CartDetialTraj.txt`
|
||||
- 证据来源：`ControllerClient.h`、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：Python 包装是否直接暴露该方法并返回 `bool`，虽然字符串存在，但公开示例未调用
|
||||
|
||||
### `IsFlyShotTrajValid`
|
||||
|
||||
- C++ 公开签名：`bool IsFlyShotTrajValid(double &time, const std::string &name, const std::string &method = "icsp", bool save_traj = false);`
|
||||
- Python 包装形态：`valid, time_sec = c.IsFlyShotTrajValid("EOL9_EAU_90", "icsp", save_traj=True)`
|
||||
- 服务端归属：`ControllerServer._IsFlyShotTrajValid`
|
||||
- 协议 / 命令线索：命令名 `IsFlyShotTrajValid` 已坐实；作用是“生成 + 合法性检查 + 返回轨迹总时长”
|
||||
- 返回值与默认值：默认 `method="icsp"`、`save_traj=false`；C++ 为 `bool + out double`；Python 为 `(bool, float)`
|
||||
- 典型工作流位置：在执行飞拍轨迹前预校验；也可与 `save_traj=True` 结合导出分析文件
|
||||
- 证据来源：`ControllerClient.h`、`UseControllerClient.py` 中的注释样例、`ControllerServer_analysis.md`、`Trajectory_generation_algorithm_analysis.md`、二进制字符串
|
||||
- 置信度：高
|
||||
- 待确认点：`method="self-adapt-icsp"` 是否被该接口完整支持，分析文档显示公开手册更强调 `icsp` / `doubles`，而示例注释里又出现 `self-adapt-icsp`，这里仍需后续抓包或真环境验证
|
||||
|
||||
## 8. 四条旧 SDK 工作流
|
||||
|
||||
### 8.1 初始化工作流
|
||||
|
||||
```text
|
||||
ControllerClient()
|
||||
-> ConnectServer("127.0.0.1", 50001)
|
||||
-> SetUpRobot("FANUC_LR_Mate_200iD") 或 SetUpRobotFromEnv(...)
|
||||
-> IsSetUp()
|
||||
-> GetName()
|
||||
-> GetDoF()
|
||||
-> SetShowTCP(...)
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- 这是最常见的仿真或真机前置流程。
|
||||
- `SetUpRobot` 是旧说明里明确要求首先执行的服务端初始化动作。
|
||||
|
||||
### 8.2 控制器状态工作流
|
||||
|
||||
```text
|
||||
SetActiveController(sim=True/False)
|
||||
-> Connect(robot_ip)
|
||||
-> EnableRobot(buffer_size=2)
|
||||
-> GetSpeedRatio() / SetSpeedRatio(...)
|
||||
-> GetTCP() / SetTCP(...)
|
||||
-> GetIO() / SetIO(...)
|
||||
-> StopMove()
|
||||
-> DisableRobot()
|
||||
-> Disconnect()
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- 真机样例会使用 `sim=False`。
|
||||
- `StopMove()` 之后，Python 示例里会再次 `EnableRobot()`，说明停止运动后常伴随重新使能。
|
||||
|
||||
### 8.3 普通轨迹工作流
|
||||
|
||||
```text
|
||||
GetJointPosition()
|
||||
-> GetPose()
|
||||
-> GetNearestIK(pose, seed)
|
||||
-> MoveJoint(home_joint)
|
||||
-> ExecuteTrajectory(waypoints, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- 输入是关节空间稀疏 waypoint。
|
||||
- `ExecuteTrajectory` 至少支持 `icsp` 与 `doubles`。
|
||||
|
||||
### 8.4 飞拍轨迹工作流
|
||||
|
||||
#### 方案 A：轨迹名已在配置中存在
|
||||
|
||||
```text
|
||||
name
|
||||
-> IsFlyShotTrajValid(name, "icsp", save_traj=True)
|
||||
-> ExecuteFlyShotTraj(name, move_to_start=True, method="icsp", save_traj=True, use_cache=False)
|
||||
-> SaveTrajInfo(name, "icsp")
|
||||
```
|
||||
|
||||
#### 方案 B：客户端动态上传飞拍轨迹
|
||||
|
||||
```text
|
||||
UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)
|
||||
-> ListFlyShotTraj()
|
||||
-> IsFlyShotTrajValid(name, "icsp", save_traj=True)
|
||||
-> ExecuteFlyShotTraj(name, move_to_start=True, method="self-adapt-icsp", save_traj=True, use_cache=True)
|
||||
-> DeleteFlyShotTraj(name)
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- `UploadFlyShotTraj` 只负责登记轨迹定义。
|
||||
- `IsFlyShotTrajValid` 负责“生成 + 合法性检查”。
|
||||
- `ExecuteFlyShotTraj` 负责“生成 + 挂接 `TrajectoryDO` + 执行”。
|
||||
- `SaveTrajInfo` 负责导出分析文件。
|
||||
|
||||
## 9. 已知高置信协议线索
|
||||
|
||||
当前能直接用于后续实现的高置信协议结论如下：
|
||||
|
||||
- 传输层为 `TCP + JSON` 风格协议，不是 HTTP/gRPC。
|
||||
- JSON 层至少存在 `method` 字段，服务端存在 `_ClientCB` 与 `_IsJsonValid`。
|
||||
- 命令通道字段已确认：
|
||||
- `GetSpeedRatio`：`MsgID = 0x2206`
|
||||
- `SetSpeedRatio`：`MsgID = 0x2207`
|
||||
- `GetIO`：`MsgID = 0x2208`
|
||||
- `SetIO`：`MsgID = 0x2209`
|
||||
- 飞拍轨迹相关额外字符串线索：
|
||||
- `StartUploadFlyShotTraj`
|
||||
- `EndUploadFlyShotTraj`
|
||||
- `GetNextListFlyShotTraj`
|
||||
- `move_to_start`
|
||||
- `use_cache`
|
||||
- `shot_flags`
|
||||
- `offset_values`
|
||||
- `addr` / `addrs`
|
||||
|
||||
## 10. 待确认问题
|
||||
|
||||
以下问题本轮故意保留，不冒充已确认结论：
|
||||
|
||||
1. `50001/TCP+JSON` 请求 JSON 的精确 envelope 结构、字段必填规则、响应包统一格式。
|
||||
2. `GetServerVersion` 在高层 JSON 中的完整请求 / 响应字段。
|
||||
3. `GetClientVersion` 的实际版本字符串来源，以及 Python 包装失败路径。
|
||||
4. `ListFlyShotTraj` 是高层一次性返回数组，还是客户端内部循环 `GetNextListFlyShotTraj` 后再拼装列表。
|
||||
5. `UploadFlyShotTraj` 是否采用开始 / 数据 / 结束的多阶段上传协议。
|
||||
6. `IsFlyShotTrajValid` 对 `self-adapt-icsp` 的真实支持边界。
|
||||
7. `SetTCP` / `GetTCP` 在高层 JSON 中是否暴露 `tcp_id` 概念。
|
||||
8. `SetActiveController` 切换控制器时是否会隐式触发 `_DisconnectAll`。
|
||||
|
||||
## 11. 后续实现使用方式
|
||||
|
||||
等继续扩展 `Flyshot.ControllerClientCompat` 时，建议按以下顺序使用本文档：
|
||||
|
||||
1. 先把 32 个 API 按本文档拆成命令表。
|
||||
2. 先实现高置信、状态简单的接口：
|
||||
- `GetServerVersion`
|
||||
- `SetUpRobot`
|
||||
- `IsSetUp`
|
||||
- `GetName`
|
||||
- `GetDoF`
|
||||
- `GetSpeedRatio`
|
||||
- `SetSpeedRatio`
|
||||
- `GetIO`
|
||||
- `SetIO`
|
||||
3. 再实现返回复杂结构的接口：
|
||||
- `GetTCP`
|
||||
- `GetJointPosition`
|
||||
- `GetPose`
|
||||
- `GetNearestIK`
|
||||
4. 最后实现飞拍轨迹相关接口，并把本文档中的“待确认问题”逐项收敛成兼容测试。
|
||||
169
docs/fanuc-socket-implementation-plan.md
Normal file
169
docs/fanuc-socket-implementation-plan.md
Normal file
@@ -0,0 +1,169 @@
|
||||
# FANUC 真机协议 Socket 通信层实现计划
|
||||
|
||||
## 上下文
|
||||
|
||||
当前 `flyshot-replacement` 项目已完成：
|
||||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
**缺失的关键环节**：`FanucControllerRuntime` 仍是纯内存状态桩，没有实际 Socket 通信。`Connect()` 只记录 IP，`ExecuteTrajectory()` 只修改内存变量，`GetJointPositions()` 返回的是上一次写入值而非真实控制器反馈。
|
||||
|
||||
## 目标
|
||||
|
||||
将 `FanucControllerRuntime` 从内存桩改造为具备真实 FANUC R30iB 通信能力的运行时，使 HTTP 层的每个指令真正下发到控制柜。
|
||||
|
||||
## 架构设计
|
||||
|
||||
### 分层结构
|
||||
|
||||
```
|
||||
LegacyHttpApiController / StatusController (HTTP 适配层，保持不动)
|
||||
↓ 调用同步接口
|
||||
IControllerRuntime / ControllerClientCompatService (兼容层，保持不动)
|
||||
↓ 调用同步接口
|
||||
FanucControllerRuntime (改造：从内存桩 → 委托给三个 Socket 客户端)
|
||||
↓ 内部持有并调度
|
||||
FanucCommandClient (TCP 10012，Req/Res 同步命令通道)
|
||||
FanucStateClient (TCP 10010，持续接收状态帧后台循环)
|
||||
FanucJ519Client (UDP 60015，8ms 周期发送 + 接收响应)
|
||||
↓ 使用现有编解码
|
||||
FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改动)
|
||||
```
|
||||
|
||||
### 关键设计决策
|
||||
|
||||
1. **接口保持同步**：`IControllerRuntime` 现有 18 个方法全为同步签名。内部 Socket I/O 采用 `Task` + `.GetAwaiter().GetResult()` 短时间阻塞，或后台线程 + 锁同步状态快照。避免一次性推翻整个兼容层。
|
||||
|
||||
2. **三个独立客户端**：每条物理通道一个类，各自管理连接生命周期，便于单独测试和故障定位。
|
||||
|
||||
3. **状态通道后台循环**：`FanucStateClient` 内部启动 `Task` 持续 `ReadAsync(90)`，解析状态帧后写入线程安全的 `ControllerStateSnapshot` 缓存。
|
||||
|
||||
4. **J519 周期发送器**：`FanucJ519Client` 内部用 `PeriodicTimer` 或 `Task.Delay` 实现约 8ms 周期的发送循环。命令通过线程安全的队列/缓冲区注入。
|
||||
|
||||
5. **RVBUSTSM 程序生命周期隐式管理**：`EnableRobot()` 时自动走 `StopProg→Reset→GetProgStatus→StartProg("RVBUSTSM")` 序列（与抓包一致）。`DisableRobot()` 时发送 `StopProg`。
|
||||
|
||||
6. **连接顺序**：`Connect()` 按顺序建立三条通道 — 先 TCP 10010（状态），再 TCP 10012（命令），最后 UDP 60015（运动）。
|
||||
|
||||
## 实现步骤
|
||||
|
||||
### Phase 1: TCP 10012 命令客户端
|
||||
|
||||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs`
|
||||
|
||||
职责：
|
||||
- `Connect(string ip, int port = 10012)` — 建立 TcpClient 连接
|
||||
- `SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body)` — 发送并等待响应
|
||||
- `SendProgramCommandAsync(uint messageId, string programName)` — 封装程序名命令
|
||||
- `Disconnect()` — 关闭连接
|
||||
- 线程安全（单个命令通道同一时间只处理一个请求）
|
||||
|
||||
需要封装的具体命令方法：
|
||||
- `StopProgramAsync(string name)` → `PackProgramCommand(0x2103, name)`
|
||||
- `ResetRobotAsync()` → `PackEmptyCommand(0x2100)`
|
||||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||||
- 用 `TcpListener` 本地模拟控制器，验证帧收发与解析
|
||||
|
||||
### Phase 2: TCP 10010 状态客户端
|
||||
|
||||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs`
|
||||
|
||||
职责：
|
||||
- `Connect(string ip, int port = 10010)` — 建立 TcpClient 连接
|
||||
- 内部启动后台 `Task` 循环 `ReadAsync(FanucStateProtocol.StateFrameLength)`
|
||||
- 每收到一帧调用 `FanucStateProtocol.ParseFrame()`
|
||||
- 将解析结果写入线程安全的最新状态缓存
|
||||
- 单帧接收超时后标记状态陈旧，不再把旧帧当作当前位姿/关节状态使用
|
||||
- EOF、坏帧、Socket 异常或超时后关闭当前连接，并按退避策略自动重连 TCP 10010
|
||||
- `GetLatestFrame()` — 返回最近一次解析的状态帧
|
||||
- `GetStatus()` — 返回连接阶段、陈旧状态、最近异常和重连次数
|
||||
- `Disconnect()` — 取消后台循环并关闭连接
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucStateClientTests.cs`
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析。
|
||||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留，不从 `[2,0,0,1]` 推断使能或运动状态。
|
||||
|
||||
### Phase 3: UDP 60015 J519 运动客户端
|
||||
|
||||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs`
|
||||
|
||||
职责：
|
||||
- `Connect(string ip, int port = 60015)` — 创建 UdpClient
|
||||
- 发送 init packet (`PackInitPacket()`)
|
||||
- 内部启动发送循环（约 8ms 周期）
|
||||
- `UpdateCommand(FanucJ519Command command)` — 原子更新下一周期要发送的命令
|
||||
- `StartMotion()` — 启动发送循环
|
||||
- `StopMotion()` — 发送 end packet，停止循环
|
||||
- 接收线程：持续 `ReceiveAsync()` 解析 132B 响应，更新反馈状态
|
||||
- `Disconnect()` — 清理
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs`
|
||||
- 用本地 UDP socket 模拟控制器收发
|
||||
|
||||
### Phase 4: 重写 FanucControllerRuntime
|
||||
|
||||
**改造文件**：`src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
|
||||
将当前内存桩替换为真实运行时：
|
||||
|
||||
- 持有三个客户端实例：`FanucCommandClient`、`FanucStateClient`、`FanucJ519Client`
|
||||
- `Connect(robotIp)` — 顺序连接 10010 → 10012 → 60015
|
||||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
||||
- `DisableRobot()` — 停止 J519，发送 StopProg
|
||||
- `Disconnect()` — 断开三条通道
|
||||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点通过 J519 逐帧发送
|
||||
- `StopMove()` — 立即停止 J519 发送循环
|
||||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
||||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||||
- `SetTcp()` / `SetSpeedRatio()` / `SetIo()` — 通过命令通道发送
|
||||
|
||||
### Phase 5: 端到端集成测试
|
||||
|
||||
**改造/新建测试**：
|
||||
- `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs` — 补充真实连接流程（可用本地模拟器）
|
||||
- `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` — 用本地 TCP/UDP 模拟器验证完整链路
|
||||
|
||||
**验证命令**：
|
||||
```bash
|
||||
cd flyshot-replacement
|
||||
dotnet build FlyshotReplacement.sln -v minimal
|
||||
dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal
|
||||
dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal
|
||||
```
|
||||
|
||||
## 风险与回退策略
|
||||
|
||||
1. **真机连接风险**：第一版 Socket 实现可能有超时/重连问题。`FanucControllerRuntime` 保留 `_simulationMode` 路径，仿真模式下仍走内存桩。
|
||||
2. **性能风险**：同步接口内部阻塞 Socket 可能影响 HTTP 并发。若实测有问题，后续将 `IControllerRuntime` 改为 async。
|
||||
3. **现场验证风险**：TCP 10012 参数命令已按逆向结论实现，但仍需在真实 R30iB 控制柜上确认默认 `tcp_id=1`、IO 类型/地址和错误码语义。
|
||||
|
||||
## 关键文件清单
|
||||
|
||||
| 文件 | 动作 |
|
||||
|------|------|
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs` | 新建 |
|
||||
| `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` | 重写 |
|
||||
| `tests/Flyshot.Core.Tests/FanucCommandClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucStateClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs` | 新建 |
|
||||
| `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` | 新建 |
|
||||
|
||||
## 下一步验证标准
|
||||
|
||||
- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接
|
||||
- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列
|
||||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点
|
||||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|
||||
@@ -0,0 +1,272 @@
|
||||
# Minimal Runtime Orchestration Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||||
|
||||
**Goal:** Build the first real execution slice after the HTTP-only refactor by routing `ExecuteTrajectory` and `ExecuteFlyShotTraj` through planning, triggering, and a new minimal FANUC runtime project instead of in-memory last-point assignment.
|
||||
|
||||
**Architecture:** Keep `Flyshot.Server.Host` as a pure HTTP adapter and keep uploaded program state in `Flyshot.ControllerClientCompat`, but move controller runtime state into a new `Flyshot.Runtime.Fanuc` project and add a focused planning/orchestration helper in `Flyshot.ControllerClientCompat`. Ordinary trajectory execution will use `ICspPlanner`; uploaded flyshot execution will use `SelfAdaptIcspPlanner` plus `ShotTimelineBuilder`, then hand the resulting `TrajectoryResult` to the runtime.
|
||||
|
||||
**Tech Stack:** C#, .NET 8, xUnit, existing `Flyshot.Core.Domain`, `Flyshot.Core.Planning`, `Flyshot.Core.Triggering`, ASP.NET Core DI.
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Add Runtime Contracts And Minimal FANUC Runtime
|
||||
|
||||
**Files:**
|
||||
- Create: `src/Flyshot.Runtime.Common/IControllerRuntime.cs`
|
||||
- Create: `src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj`
|
||||
- Create: `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||||
- Modify: `FlyshotReplacement.sln`
|
||||
- Modify: `src/Flyshot.Server.Host/Flyshot.Server.Host.csproj`
|
||||
- Modify: `src/Flyshot.Server.Host/Program.cs`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing runtime test**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
|
||||
{
|
||||
var runtime = new FanucControllerRuntime();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
runtime.SetActiveController(sim: false);
|
||||
runtime.Connect("192.168.10.101");
|
||||
runtime.EnableRobot(bufferSize: 2);
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(1.2),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
Assert.Equal("Connected", snapshot.ConnectionState);
|
||||
Assert.False(snapshot.IsInMotion);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because `FanucControllerRuntime` and `IControllerRuntime` do not exist.
|
||||
|
||||
- [x] **Step 3: Write the minimal runtime contracts and implementation**
|
||||
|
||||
```csharp
|
||||
public interface IControllerRuntime
|
||||
{
|
||||
void ResetRobot(RobotProfile robot, string robotName);
|
||||
void SetActiveController(bool sim);
|
||||
void Connect(string robotIp);
|
||||
void Disconnect();
|
||||
void EnableRobot(int bufferSize);
|
||||
void DisableRobot();
|
||||
void StopMove();
|
||||
double GetSpeedRatio();
|
||||
void SetSpeedRatio(double ratio);
|
||||
IReadOnlyList<double> GetTcp();
|
||||
void SetTcp(double x, double y, double z);
|
||||
bool GetIo(int port, string ioType);
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
IReadOnlyList<double> GetPose();
|
||||
ControllerStateSnapshot GetSnapshot();
|
||||
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
|
||||
}
|
||||
```
|
||||
|
||||
```csharp
|
||||
public sealed class FanucControllerRuntime : IControllerRuntime
|
||||
{
|
||||
// Stage-1 runtime: owns controller state in one place so later sockets can replace internals without rewriting compat service.
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run test to verify it passes**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
### Task 2: Add Planning And Triggering Orchestration For Execution
|
||||
|
||||
**Files:**
|
||||
- Create: `src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs`
|
||||
- Create: `src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/Flyshot.ControllerClientCompat.csproj`
|
||||
- Modify: `tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing orchestration tests**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
orchestrator.PlanOrdinaryTrajectory(robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
|
||||
Assert.True(bundle.Result.IsValid);
|
||||
Assert.Single(bundle.Result.ShotEvents);
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run tests to verify they fail**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientTrajectoryOrchestrator -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because the orchestrator types do not exist.
|
||||
|
||||
- [x] **Step 3: Write the minimal orchestration layer**
|
||||
|
||||
```csharp
|
||||
public sealed class PlannedExecutionBundle
|
||||
{
|
||||
public PlannedExecutionBundle(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, TrajectoryResult result)
|
||||
{
|
||||
PlannedTrajectory = plannedTrajectory;
|
||||
ShotTimeline = shotTimeline;
|
||||
Result = result;
|
||||
}
|
||||
|
||||
public PlannedTrajectory PlannedTrajectory { get; }
|
||||
public ShotTimeline ShotTimeline { get; }
|
||||
public TrajectoryResult Result { get; }
|
||||
}
|
||||
```
|
||||
|
||||
```csharp
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> waypoints) { ... }
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded) { ... }
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run tests to verify they pass**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientTrajectoryOrchestrator -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
### Task 3: Rewire ControllerClientCompatService To Runtime + Orchestrator
|
||||
|
||||
**Files:**
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs`
|
||||
- Modify: `src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs`
|
||||
- Modify: `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs`
|
||||
- Modify: `tests/Flyshot.Server.IntegrationTests/ControllerClientCompatRegistrationTests.cs`
|
||||
- Test: `tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs`
|
||||
- Test: `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs`
|
||||
|
||||
- [x] **Step 1: Write the failing compat-service test**
|
||||
|
||||
```csharp
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
|
||||
{
|
||||
var service = TestRobotFactory.CreateCompatService();
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
|
||||
Assert.Throws<ArgumentException>(() =>
|
||||
service.ExecuteTrajectory(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]));
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because current service still treats ordinary execution as "move to last waypoint".
|
||||
|
||||
- [x] **Step 3: Rewire service to the runtime and orchestrator**
|
||||
|
||||
```csharp
|
||||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||||
{
|
||||
private readonly IControllerRuntime _runtime;
|
||||
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
|
||||
|
||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints);
|
||||
_runtime.ExecuteTrajectory(bundle.Result, bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions);
|
||||
}
|
||||
|
||||
public void ExecuteTrajectoryByName(string name)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
var uploaded = RequireUploadedTrajectory(name);
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
_runtime.ExecuteTrajectory(bundle.Result, bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions);
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
- [x] **Step 4: Run focused tests to verify green**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter "ControllerClientCompatService|ControllerClientTrajectoryOrchestrator|FanucControllerRuntime" -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
- [x] **Step 5: Run integration verification**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS, with existing HTTP compatibility tests still green.
|
||||
|
||||
### Task 4: Verify Solution Build And Update Progress Docs
|
||||
|
||||
**Files:**
|
||||
- Modify: `README.md`
|
||||
- Modify: `AGENTS.md`
|
||||
- Modify: `docs/controller-client-api-compatibility-requirements.md`
|
||||
|
||||
- [x] **Step 1: Update docs to reflect the new stage**
|
||||
|
||||
```markdown
|
||||
- [x] 落地最小 FANUC 运行时骨架
|
||||
- [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime
|
||||
- [x] 落地 Web 状态页
|
||||
- [x] 固化 10010 / 10012 / 60015 FANUC 基础协议帧编解码
|
||||
- [ ] 落地真实 10010 / 10012 / 60015 Socket 通讯与现场联调
|
||||
```
|
||||
|
||||
- [x] **Step 2: Run final build**
|
||||
|
||||
Run: `dotnet build FlyshotReplacement.sln --no-restore -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS with 0 errors.
|
||||
@@ -0,0 +1,17 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 HTTP-only ControllerClient 兼容层的基础配置。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置对外暴露的服务端版本号。
|
||||
/// </summary>
|
||||
public string ServerVersion { get; set; } = "flyshot-replacement-controller-client-compat/0.1.0";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置父工作区根目录；为空时由运行时自动推断。
|
||||
/// </summary>
|
||||
public string? WorkspaceRoot { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,85 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧版 ControllerClient 的机器人名称，解析当前 replacement 仓库支持的真实模型文件。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatRobotCatalog
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存当前现场支持的机器人名称到模型相对路径映射。
|
||||
/// </summary>
|
||||
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
||||
{
|
||||
["FANUC_LR_Mate_200iD"] = Path.Combine("FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot"),
|
||||
["FANUC_LR_Mate_200iD_7L"] = Path.Combine("FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot")
|
||||
};
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化机器人兼容目录解析器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置。</param>
|
||||
/// <param name="robotModelLoader">.robot 文件加载器。</param>
|
||||
public ControllerClientCompatRobotCatalog(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端的机器人名称加载对应模型。
|
||||
/// </summary>
|
||||
/// <param name="robotName">旧客户端传入的机器人名称。</param>
|
||||
/// <param name="accLimitScale">RobotConfig.json 中的加速度倍率。</param>
|
||||
/// <param name="jerkLimitScale">RobotConfig.json 中的 jerk 倍率。</param>
|
||||
/// <returns>兼容层加载出的机器人模型。</returns>
|
||||
public RobotProfile LoadProfile(string robotName, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotName))
|
||||
{
|
||||
throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
|
||||
}
|
||||
|
||||
if (!SupportedRobotModelMap.TryGetValue(robotName, out var modelRelativePath))
|
||||
{
|
||||
throw new InvalidOperationException($"Unsupported robot name: {robotName}");
|
||||
}
|
||||
|
||||
var workspaceRoot = ResolveWorkspaceRoot();
|
||||
var modelPath = Path.Combine(workspaceRoot, modelRelativePath);
|
||||
return _robotModelLoader.LoadProfile(modelPath, accLimitScale, jerkLimitScale);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析父工作区根目录，优先使用显式配置。
|
||||
/// </summary>
|
||||
/// <returns>包含 `FlyingShot/` 与 `Rvbust/` 的父工作区根目录。</returns>
|
||||
private string ResolveWorkspaceRoot()
|
||||
{
|
||||
if (!string.IsNullOrWhiteSpace(_options.WorkspaceRoot))
|
||||
{
|
||||
return Path.GetFullPath(_options.WorkspaceRoot);
|
||||
}
|
||||
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
// 宿主和测试都从 replacement 仓库内启动；找到 sln 后回退一级就是父工作区根目录。
|
||||
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,646 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 在宿主进程内实现 HTTP-only ControllerClient 兼容语义，并把控制器状态委托给运行时。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
|
||||
private readonly IControllerRuntime _runtime;
|
||||
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
private readonly IFlyshotTrajectoryStore _trajectoryStore;
|
||||
private RobotProfile? _activeRobotProfile;
|
||||
private string? _configuredRobotName;
|
||||
private CompatibilityRobotSettings? _robotSettings;
|
||||
private string? _connectedServerIp;
|
||||
private int _connectedServerPort;
|
||||
private bool _showTcp = true;
|
||||
private double _showTcpAxisLength = 0.1;
|
||||
private int _showTcpAxisSize = 2;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置。</param>
|
||||
/// <param name="robotCatalog">机器人模型目录。</param>
|
||||
/// <param name="runtime">控制器运行时。</param>
|
||||
/// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器。</param>
|
||||
/// <param name="trajectoryStore">已上传轨迹持久化存储。</param>
|
||||
public ControllerClientCompatService(
|
||||
ControllerClientCompatOptions options,
|
||||
ControllerClientCompatRobotCatalog robotCatalog,
|
||||
IControllerRuntime runtime,
|
||||
ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
|
||||
RobotConfigLoader configLoader,
|
||||
IFlyshotTrajectoryStore trajectoryStore)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotCatalog = robotCatalog ?? throw new ArgumentNullException(nameof(robotCatalog));
|
||||
_runtime = runtime ?? throw new ArgumentNullException(nameof(runtime));
|
||||
_trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
_trajectoryStore = trajectoryStore ?? throw new ArgumentNullException(nameof(trajectoryStore));
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string ServerVersion => _options.ServerVersion;
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsSetUp
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _activeRobotProfile is not null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运行时是否处于运动态。
|
||||
/// </summary>
|
||||
public bool IsInMotion => _runtime.GetSnapshot().IsInMotion;
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ConnectServer(string serverIp, int port)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(serverIp))
|
||||
{
|
||||
throw new ArgumentException("服务端 IP 不能为空。", nameof(serverIp));
|
||||
}
|
||||
|
||||
if (port <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(port), "端口必须大于 0。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
// HTTP-only 阶段仍记录旧客户端期望的 50001 地址，便于后续 TCP 入口恢复时复用状态。
|
||||
_connectedServerIp = serverIp;
|
||||
_connectedServerPort = port;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetServerVersion()
|
||||
{
|
||||
return ServerVersion;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetClientVersion()
|
||||
{
|
||||
return "flyshot-replacement-controller-client-compat/0.1.0";
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetUpRobot(string robotName)
|
||||
{
|
||||
var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings();
|
||||
var robotProfile = _robotCatalog.LoadProfile(
|
||||
robotName,
|
||||
robotSettings.AccLimitScale,
|
||||
robotSettings.JerkLimitScale);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
// 机器人重新初始化时同步重置运行时和上传轨迹目录，保持旧服务初始化语义。
|
||||
_configuredRobotName = robotName;
|
||||
_activeRobotProfile = robotProfile;
|
||||
_uploadedTrajectories.Clear();
|
||||
_runtime.ResetRobot(robotProfile, robotName);
|
||||
_robotSettings = robotSettings;
|
||||
|
||||
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
|
||||
var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _);
|
||||
foreach (var saved in savedTrajectories)
|
||||
{
|
||||
_uploadedTrajectories[saved.Key] = saved.Value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetUpRobotFromEnv(string envFile)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(envFile))
|
||||
{
|
||||
throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
|
||||
}
|
||||
|
||||
throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetShowTcp(bool isShow, double axisLength, int axisSize)
|
||||
{
|
||||
if (axisLength <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。");
|
||||
}
|
||||
|
||||
if (axisSize <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
|
||||
// 当前无 GUI 渲染层，先保存显示参数，保证旧 SDK 参数不会在 HTTP 边界丢失。
|
||||
_showTcp = isShow;
|
||||
_showTcpAxisLength = axisLength;
|
||||
_showTcpAxisSize = axisSize;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetActiveController(sim);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotIp))
|
||||
{
|
||||
throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Connect(robotIp);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Disconnect();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.EnableRobot(bufferSize);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.DisableRobot();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.StopMove();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public ControllerStateSnapshot GetControllerSnapshot()
|
||||
{
|
||||
return _runtime.GetSnapshot();
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public double GetSpeedRatio()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetSpeedRatio();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetSpeedRatio(double ratio)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetSpeedRatio(ratio);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetIo(int port, bool value, string ioType)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetIo(port, value, ioType);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool GetIo(int port, string ioType)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetIo(port, ioType);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
ArgumentNullException.ThrowIfNull(seed);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("位姿必须是 [x,y,z,qx,qy,qz,qw] 七元数组。", nameof(pose));
|
||||
}
|
||||
|
||||
if (seed.Count != GetDegreesOfFreedom())
|
||||
{
|
||||
throw new ArgumentException("seed 关节数量必须与机器人自由度一致。", nameof(seed));
|
||||
}
|
||||
|
||||
throw new NotSupportedException("GetNearestIK 尚未接入逆解求解器。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetTcp(double x, double y, double z)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.SetTcp(x, y, z);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetTcp()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetTcp();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetJointPositions()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetJointPositions();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void MoveJoint(IReadOnlyList<double> jointPositions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(jointPositions);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), jointPositions);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
if (waypoints.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options);
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetPose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRuntimeEnabled();
|
||||
_uploadedTrajectories[trajectory.Name] = trajectory;
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<string> ListTrajectoryNames()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _uploadedTrajectories.Keys.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
|
||||
{
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
if (trajectory.Waypoints.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||||
}
|
||||
|
||||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, RequireRobotSettings());
|
||||
if (options.MoveToStart)
|
||||
{
|
||||
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions);
|
||||
}
|
||||
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
|
||||
_ = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(saveTrajectory: true, method: method),
|
||||
RequireRobotSettings());
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||||
RequireRobotSettings());
|
||||
|
||||
duration = bundle.Result.Duration;
|
||||
return bundle.Result.IsValid;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DeleteTrajectory(string name)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (!_uploadedTrajectories.Remove(name))
|
||||
{
|
||||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||||
}
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
_trajectoryStore.Delete(robotName, name);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public string GetRobotName()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public int GetDegreesOfFreedom()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _activeRobotProfile?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人配置，未初始化时抛出兼容错误。
|
||||
/// </summary>
|
||||
/// <returns>当前机器人配置。</returns>
|
||||
private RobotProfile RequireActiveRobot()
|
||||
{
|
||||
return _activeRobotProfile ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人兼容配置；未加载旧配置时回退到现场默认值。
|
||||
/// </summary>
|
||||
/// <returns>当前机器人配置。</returns>
|
||||
private CompatibilityRobotSettings RequireRobotSettings()
|
||||
{
|
||||
return _robotSettings ?? CreateDefaultRobotSettings();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验机器人已经完成初始化。
|
||||
/// </summary>
|
||||
private void EnsureRobotSetup()
|
||||
{
|
||||
_ = RequireActiveRobot();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验运行时已经处于可执行状态。
|
||||
/// </summary>
|
||||
private void EnsureRuntimeEnabled()
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (!_runtime.GetSnapshot().IsEnabled)
|
||||
{
|
||||
throw new InvalidOperationException("Robot has not been enabled.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 MoveJoint 直达运行时所需的最小合法轨迹结果。
|
||||
/// </summary>
|
||||
/// <returns>可立即执行的轨迹结果。</returns>
|
||||
private static TrajectoryResult CreateImmediateMoveResult()
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.Zero,
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 1,
|
||||
plannedWaypointCount: 1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 尝试从工作区加载旧版 RobotConfig.json 获取机器人配置；失败时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>加载到的机器人配置，或 null。</returns>
|
||||
private CompatibilityRobotSettings? TryLoadRobotSettings()
|
||||
{
|
||||
try
|
||||
{
|
||||
var workspaceRoot = !string.IsNullOrWhiteSpace(_options.WorkspaceRoot)
|
||||
? Path.GetFullPath(_options.WorkspaceRoot)
|
||||
: ResolveWorkspaceRootFromBaseDirectory();
|
||||
|
||||
var configPath = PathCompatibility.ResolveConfigPath("RobotConfig.json", workspaceRoot);
|
||||
var loaded = _configLoader.Load(configPath, workspaceRoot);
|
||||
return loaded.Robot;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造与旧现场默认行为一致的机器人兼容配置。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: false,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 2,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前程序基目录向上搜索 FlyshotReplacement.sln 以推断工作区根目录。
|
||||
/// </summary>
|
||||
/// <returns>父工作区根目录。</returns>
|
||||
private static string ResolveWorkspaceRootFromBaseDirectory()
|
||||
{
|
||||
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,41 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 提供 ControllerClient HTTP 兼容层的依赖注入注册入口。
|
||||
/// </summary>
|
||||
public static class ControllerClientCompatServiceCollectionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// 将 HTTP-only 的 ControllerClient 兼容服务注册到当前宿主。
|
||||
/// </summary>
|
||||
/// <param name="services">当前宿主服务集合。</param>
|
||||
/// <param name="configuration">宿主配置根。</param>
|
||||
/// <returns>同一服务集合，便于链式调用。</returns>
|
||||
public static IServiceCollection AddControllerClientCompat(this IServiceCollection services, IConfiguration configuration)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(services);
|
||||
ArgumentNullException.ThrowIfNull(configuration);
|
||||
|
||||
services
|
||||
.AddOptions<ControllerClientCompatOptions>()
|
||||
.Bind(configuration.GetSection("ControllerClientCompat"));
|
||||
|
||||
services.AddSingleton(static serviceProvider => serviceProvider.GetRequiredService<IOptions<ControllerClientCompatOptions>>().Value);
|
||||
services.AddSingleton<RobotModelLoader>();
|
||||
services.AddSingleton<RobotConfigLoader>();
|
||||
services.AddSingleton<ControllerClientCompatRobotCatalog>();
|
||||
services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
|
||||
services.AddSingleton<IFlyshotTrajectoryStore, JsonFlyshotTrajectoryStore>();
|
||||
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
|
||||
services.AddSingleton<IControllerClientCompatService, ControllerClientCompatService>();
|
||||
|
||||
return services;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 保存一条已上传到兼容层内存目录中的飞拍轨迹，供 HTTP API 层列出、执行和删除。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatUploadedTrajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条飞拍轨迹快照，并立即复制所有数组，避免调用方后续原地修改。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点集合。</param>
|
||||
/// <param name="shotFlags">拍摄标志集合。</param>
|
||||
/// <param name="offsetValues">偏移周期集合。</param>
|
||||
/// <param name="addressGroups">地址组集合。</param>
|
||||
public ControllerClientCompatUploadedTrajectory(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
ArgumentNullException.ThrowIfNull(shotFlags);
|
||||
ArgumentNullException.ThrowIfNull(offsetValues);
|
||||
ArgumentNullException.ThrowIfNull(addressGroups);
|
||||
|
||||
Name = name;
|
||||
Waypoints = waypoints.Select(static waypoint => waypoint.ToArray()).ToArray();
|
||||
ShotFlags = shotFlags.ToArray();
|
||||
OffsetValues = offsetValues.ToArray();
|
||||
AddressGroups = addressGroups.Select(static group => group.ToArray()).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹名称。
|
||||
/// </summary>
|
||||
public string Name { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取已复制的关节路点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double[]> Waypoints { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取拍摄标志集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<bool> ShotFlags { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取偏移周期集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int> OffsetValues { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取地址组集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int[]> AddressGroups { get; }
|
||||
}
|
||||
@@ -0,0 +1,297 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
private readonly ICspPlanner _icspPlanner = new();
|
||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new();
|
||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||
|
||||
/// <summary>
|
||||
/// 对普通轨迹执行 ICSP 规划。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="waypoints">普通轨迹关节路点。</param>
|
||||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||||
TrajectoryExecutionOptions? options = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
|
||||
var program = CreateProgram(
|
||||
name: "ordinary-trajectory",
|
||||
waypoints: waypoints,
|
||||
shotFlags: Enumerable.Repeat(false, waypoints.Count),
|
||||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||||
|
||||
var method = ParseOrdinaryMethod(options.Method);
|
||||
var request = new TrajectoryRequest(
|
||||
robot: robot,
|
||||
program: program,
|
||||
method: method,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
|
||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions? options = null,
|
||||
CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
settings ??= CreateDefaultRobotSettings();
|
||||
|
||||
var program = CreateProgram(
|
||||
name: uploaded.Name,
|
||||
waypoints: uploaded.Waypoints,
|
||||
shotFlags: uploaded.ShotFlags,
|
||||
offsetValues: uploaded.OffsetValues,
|
||||
addressGroups: uploaded.AddressGroups);
|
||||
|
||||
var method = ParseFlyshotMethod(options.Method);
|
||||
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||||
return new PlannedExecutionBundle(
|
||||
cachedBundle.PlannedTrajectory,
|
||||
cachedBundle.ShotTimeline,
|
||||
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
|
||||
}
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: robot,
|
||||
program: program,
|
||||
method: method,
|
||||
moveToStart: options.MoveToStart,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||||
useCache: options.UseCache);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method, settings);
|
||||
var shotTimeline = _shotTimelineBuilder.Build(
|
||||
plannedTrajectory,
|
||||
holdCycles: settings.IoKeepCycles,
|
||||
samplePeriod: robot.ServoPeriod,
|
||||
useDo: settings.UseDo);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
|
||||
if (options.UseCache)
|
||||
{
|
||||
_flyshotCache[cacheKey] = bundle;
|
||||
}
|
||||
|
||||
return bundle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按普通轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseFlyshotMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按领域枚举分派到当前已经落地的规划器。
|
||||
/// </summary>
|
||||
/// <param name="request">规划请求。</param>
|
||||
/// <param name="method">规划方法。</param>
|
||||
/// <returns>规划轨迹。</returns>
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
|
||||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 归一化旧 SDK 的 method 字符串。
|
||||
/// </summary>
|
||||
/// <param name="method">原始方法名。</param>
|
||||
/// <returns>小写短横线方法名。</returns>
|
||||
private static string NormalizeMethod(string method)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(method))
|
||||
{
|
||||
return "icsp";
|
||||
}
|
||||
|
||||
return method.Trim().ToLowerInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置。</param>
|
||||
/// <param name="uploaded">上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <returns>缓存键。</returns>
|
||||
private static string CreateFlyshotCacheKey(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions options,
|
||||
CompatibilityRobotSettings settings)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
hash.Add(uploaded.Name, StringComparer.Ordinal);
|
||||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||||
hash.Add(options.MoveToStart);
|
||||
hash.Add(options.SaveTrajectory);
|
||||
hash.Add(settings.UseDo);
|
||||
hash.Add(settings.IoKeepCycles);
|
||||
hash.Add(settings.AdaptIcspTryNum);
|
||||
|
||||
foreach (var waypoint in uploaded.Waypoints)
|
||||
{
|
||||
foreach (var value in waypoint)
|
||||
{
|
||||
hash.Add(value);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var flag in uploaded.ShotFlags)
|
||||
{
|
||||
hash.Add(flag);
|
||||
}
|
||||
|
||||
foreach (var offset in uploaded.OffsetValues)
|
||||
{
|
||||
hash.Add(offset);
|
||||
}
|
||||
|
||||
foreach (var group in uploaded.AddressGroups)
|
||||
{
|
||||
foreach (var address in group)
|
||||
{
|
||||
hash.Add(address);
|
||||
}
|
||||
}
|
||||
|
||||
return hash.ToHashCode().ToString("X8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人兼容配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 0,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点。</param>
|
||||
/// <param name="shotFlags">拍照标志。</param>
|
||||
/// <param name="offsetValues">偏移周期。</param>
|
||||
/// <param name="addressGroups">IO 地址组。</param>
|
||||
/// <returns>领域层飞拍程序。</returns>
|
||||
private static FlyshotProgram CreateProgram(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: name,
|
||||
waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
|
||||
shotFlags: shotFlags.ToArray(),
|
||||
offsetValues: offsetValues.ToArray(),
|
||||
addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">触发时间轴。</param>
|
||||
/// <returns>运行时执行结果描述。</returns>
|
||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
|
||||
{
|
||||
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
plannedTrajectory,
|
||||
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: plannedTrajectory.OriginalProgram.Name,
|
||||
method: plannedTrajectory.Method,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
|
||||
shotEvents: shotTimeline.ShotEvents,
|
||||
triggerTimeline: shotTimeline.TriggerTimeline,
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: usedCache,
|
||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<FrameworkReference Include="Microsoft.AspNetCore.App" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,46 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteFlyShotTraj`。
|
||||
/// </summary>
|
||||
public sealed class FlyshotExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化飞拍轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
|
||||
/// <param name="method">轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
||||
public FlyshotExecutionOptions(
|
||||
bool moveToStart = true,
|
||||
string method = "icsp",
|
||||
bool saveTrajectory = true,
|
||||
bool useCache = true)
|
||||
{
|
||||
MoveToStart = moveToStart;
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
UseCache = useCache;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取执行前是否自动移动到轨迹起点。
|
||||
/// </summary>
|
||||
public bool MoveToStart { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否优先复用已规划轨迹缓存。
|
||||
/// </summary>
|
||||
public bool UseCache { get; }
|
||||
}
|
||||
231
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
231
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
@@ -0,0 +1,231 @@
|
||||
using System.Text.Json;
|
||||
using System.Text.Json.Nodes;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 定义已上传飞拍轨迹的持久化存储契约。
|
||||
/// </summary>
|
||||
public interface IFlyshotTrajectoryStore
|
||||
{
|
||||
/// <summary>
|
||||
/// 将单条轨迹持久化到本地 JSON，同时更新所属机器人配置段。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||||
void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory);
|
||||
|
||||
/// <summary>
|
||||
/// 从本地 JSON 删除指定名称的轨迹。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||||
void Delete(string robotName, string trajectoryName);
|
||||
|
||||
/// <summary>
|
||||
/// 加载指定机器人名下所有已持久化的轨迹，并回传保存时的机器人配置。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="settings">输出保存时的机器人配置；若文件不存在或解析失败则为 null。</param>
|
||||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||||
IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用与旧版 RobotConfig.json 一致的 JSON 格式持久化飞拍轨迹和机器人配置。
|
||||
/// </summary>
|
||||
public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化基于 JSON 文件的轨迹存储。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位工作区根目录。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。</param>
|
||||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(settings);
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
var path = ResolveStorePath(robotName);
|
||||
var directory = Path.GetDirectoryName(path)!;
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
JsonObject root;
|
||||
if (File.Exists(path))
|
||||
{
|
||||
var existingJson = File.ReadAllText(path);
|
||||
root = JsonNode.Parse(existingJson)?.AsObject() ?? new JsonObject();
|
||||
}
|
||||
else
|
||||
{
|
||||
root = new JsonObject();
|
||||
}
|
||||
|
||||
// 更新 robot 配置段，保持与旧版 RobotConfig.json 字段名一致。
|
||||
root["robot"] = SerializeRobotSettings(settings);
|
||||
|
||||
// 确保 flying_shots 节点存在。
|
||||
if (!root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) || flyingShotsNode is not JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj = new JsonObject();
|
||||
root["flying_shots"] = flyingShotsObj;
|
||||
}
|
||||
|
||||
flyingShotsObj[trajectory.Name] = SerializeTrajectory(trajectory);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
var path = ResolveStorePath(robotName);
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var existingJson = File.ReadAllText(path);
|
||||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||||
if (root is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj.Remove(trajectoryName);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
var path = ResolveStorePath(robotName);
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var workspaceRoot = ResolveWorkspaceRoot();
|
||||
var loaded = _configLoader.Load(path, workspaceRoot);
|
||||
settings = loaded.Robot;
|
||||
|
||||
var dict = new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
foreach (var program in loaded.Programs)
|
||||
{
|
||||
var traj = new ControllerClientCompatUploadedTrajectory(
|
||||
name: program.Value.Name,
|
||||
waypoints: program.Value.Waypoints.Select(static wp => wp.Positions),
|
||||
shotFlags: program.Value.ShotFlags,
|
||||
offsetValues: program.Value.OffsetValues,
|
||||
addressGroups: program.Value.AddressGroups.Select(static g => g.Addresses));
|
||||
dict[program.Key] = traj;
|
||||
}
|
||||
|
||||
return dict;
|
||||
}
|
||||
catch
|
||||
{
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把机器人兼容配置序列化为 JSON 对象，字段名与旧版 RobotConfig.json 一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeRobotSettings(CompatibilityRobotSettings settings)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["use_do"] = JsonValue.Create(settings.UseDo),
|
||||
["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses),
|
||||
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
||||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把已上传轨迹序列化为 JSON 对象，字段名与旧版 RobotConfig.json 的 flying_shots 节点一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["traj_waypoints"] = JsonSerializer.SerializeToNode(trajectory.Waypoints),
|
||||
["shot_flags"] = JsonSerializer.SerializeToNode(trajectory.ShotFlags),
|
||||
["offset_values"] = JsonSerializer.SerializeToNode(trajectory.OffsetValues),
|
||||
["addr"] = JsonSerializer.SerializeToNode(trajectory.AddressGroups)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析当前机器人对应的持久化文件路径。
|
||||
/// </summary>
|
||||
private string ResolveStorePath(string robotName)
|
||||
{
|
||||
var workspaceRoot = ResolveWorkspaceRoot();
|
||||
var storeDir = Path.Combine(workspaceRoot, "flyshot-replacement", "TrajectoryStore");
|
||||
return Path.Combine(storeDir, $"{robotName}_trajectories.json");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析父工作区根目录，优先使用显式配置。
|
||||
/// </summary>
|
||||
private string ResolveWorkspaceRoot()
|
||||
{
|
||||
if (!string.IsNullOrWhiteSpace(_options.WorkspaceRoot))
|
||||
{
|
||||
return Path.GetFullPath(_options.WorkspaceRoot);
|
||||
}
|
||||
|
||||
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,226 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 HTTP-only 兼容层对外暴露的 ControllerClient 语义服务接口。
|
||||
/// </summary>
|
||||
public interface IControllerClientCompatService
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取当前兼容层对外报告的服务端版本号。
|
||||
/// </summary>
|
||||
string ServerVersion { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已经完成机器人初始化。
|
||||
/// </summary>
|
||||
bool IsSetUp { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 保存当前调用方期望连接的 replacement 服务端地址。
|
||||
/// </summary>
|
||||
/// <param name="serverIp">客户端传入的服务端 IP。</param>
|
||||
/// <param name="port">客户端传入的服务端端口。</param>
|
||||
void ConnectServer(string serverIp, int port);
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容服务端版本号。
|
||||
/// </summary>
|
||||
/// <returns>服务端版本号。</returns>
|
||||
string GetServerVersion();
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容客户端版本号。
|
||||
/// </summary>
|
||||
/// <returns>客户端版本号。</returns>
|
||||
string GetClientVersion();
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端使用的机器人名称完成机器人初始化。
|
||||
/// </summary>
|
||||
/// <param name="robotName">机器人名称。</param>
|
||||
void SetUpRobot(string robotName);
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端传入的环境文件完成机器人初始化。
|
||||
/// </summary>
|
||||
/// <param name="envFile">环境文件路径。</param>
|
||||
void SetUpRobotFromEnv(string envFile);
|
||||
|
||||
/// <summary>
|
||||
/// 设置是否显示 TCP 坐标轴。
|
||||
/// </summary>
|
||||
/// <param name="isShow">是否显示 TCP。</param>
|
||||
/// <param name="axisLength">坐标轴长度。</param>
|
||||
/// <param name="axisSize">坐标轴线宽。</param>
|
||||
void SetShowTcp(bool isShow, double axisLength, int axisSize);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前激活的控制器类型。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否为仿真控制器。</param>
|
||||
void SetActiveController(bool sim);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经建立连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经断开。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人进入使能态。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人退出使能态。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动状态。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前控制器运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器运行时状态快照。</returns>
|
||||
ControllerStateSnapshot GetControllerSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>当前速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 写入兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="value">IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 读取兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>缓存中的 IO 值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 按给定位姿和 seed 计算最近 IK。
|
||||
/// </summary>
|
||||
/// <param name="pose">目标位姿数组。</param>
|
||||
/// <param name="seed">IK seed 关节数组。</param>
|
||||
/// <returns>IK 结果关节数组。</returns>
|
||||
IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed);
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 数组。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>关节位置数组。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前关节位置。
|
||||
/// </summary>
|
||||
/// <param name="jointPositions">目标关节位置。</param>
|
||||
void MoveJoint(IReadOnlyList<double> jointPositions);
|
||||
|
||||
/// <summary>
|
||||
/// 执行普通轨迹。
|
||||
/// </summary>
|
||||
/// <param name="waypoints">轨迹路点集合。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前末端位姿快照。
|
||||
/// </summary>
|
||||
/// <returns>位姿数组。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 上传一条飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="trajectory">飞拍轨迹。</param>
|
||||
void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory);
|
||||
|
||||
/// <summary>
|
||||
/// 列出当前已上传的飞拍轨迹名称。
|
||||
/// </summary>
|
||||
/// <returns>轨迹名称列表。</returns>
|
||||
IReadOnlyList<string> ListTrajectoryNames();
|
||||
|
||||
/// <summary>
|
||||
/// 执行指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="options">飞拍执行参数。</param>
|
||||
void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 保存指定飞拍轨迹的轨迹信息。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
void SaveTrajectoryInfo(string name, string method = "icsp");
|
||||
|
||||
/// <summary>
|
||||
/// 检查指定飞拍轨迹是否可执行。
|
||||
/// </summary>
|
||||
/// <param name="duration">输出规划轨迹总时长。</param>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <returns>轨迹是否有效。</returns>
|
||||
bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false);
|
||||
|
||||
/// <summary>
|
||||
/// 删除指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
void DeleteTrajectory(string name);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前配置过的机器人名称。
|
||||
/// </summary>
|
||||
/// <returns>机器人名称。</returns>
|
||||
string GetRobotName();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前机器人自由度。
|
||||
/// </summary>
|
||||
/// <returns>机器人自由度。</returns>
|
||||
int GetDegreesOfFreedom();
|
||||
}
|
||||
39
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
39
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
@@ -0,0 +1,39 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示兼容层执行轨迹前生成的完整规划结果包。
|
||||
/// </summary>
|
||||
public sealed class PlannedExecutionBundle
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份执行规划结果包。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">飞拍触发时间轴。</param>
|
||||
/// <param name="result">对运行时和监控层暴露的规划结果。</param>
|
||||
public PlannedExecutionBundle(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, TrajectoryResult result)
|
||||
{
|
||||
PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory));
|
||||
ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline));
|
||||
Result = result ?? throw new ArgumentNullException(nameof(result));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划后的轨迹。
|
||||
/// </summary>
|
||||
public PlannedTrajectory PlannedTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞拍触发时间轴。
|
||||
/// </summary>
|
||||
public ShotTimeline ShotTimeline { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取运行时可消费的规划结果。
|
||||
/// </summary>
|
||||
public TrajectoryResult Result { get; }
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示普通轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteTrajectory`。
|
||||
/// </summary>
|
||||
public sealed class TrajectoryExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化普通轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="method">轨迹生成方法，支持 `icsp` 或 `doubles`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
}
|
||||
@@ -18,7 +18,8 @@ public sealed class ControllerStateSnapshot
|
||||
double speedRatio,
|
||||
IEnumerable<double>? jointPositions = null,
|
||||
IEnumerable<double>? cartesianPose = null,
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null)
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null,
|
||||
IEnumerable<uint>? stateTailWords = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -34,6 +35,7 @@ public sealed class ControllerStateSnapshot
|
||||
var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty<RuntimeAlarm>();
|
||||
var copiedStateTailWords = stateTailWords?.ToArray() ?? Array.Empty<uint>();
|
||||
|
||||
CapturedAt = capturedAt;
|
||||
ConnectionState = connectionState;
|
||||
@@ -43,6 +45,7 @@ public sealed class ControllerStateSnapshot
|
||||
JointPositions = copiedJointPositions;
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -92,4 +95,10 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("activeAlarms")]
|
||||
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 10010 状态帧尾部原始状态字，仅用于诊断，不直接推断运行语义。
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
}
|
||||
|
||||
@@ -21,7 +21,8 @@ public sealed class TrajectoryResult
|
||||
string? failureReason,
|
||||
bool usedCache,
|
||||
int originalWaypointCount,
|
||||
int plannedWaypointCount)
|
||||
int plannedWaypointCount,
|
||||
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
@@ -51,6 +52,7 @@ public sealed class TrajectoryResult
|
||||
var copiedShotEvents = shotEvents.ToArray();
|
||||
var copiedTriggerTimeline = triggerTimeline.ToArray();
|
||||
var copiedArtifacts = artifacts.ToArray();
|
||||
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
|
||||
ProgramName = programName;
|
||||
Method = method;
|
||||
@@ -63,6 +65,7 @@ public sealed class TrajectoryResult
|
||||
UsedCache = usedCache;
|
||||
OriginalWaypointCount = originalWaypointCount;
|
||||
PlannedWaypointCount = plannedWaypointCount;
|
||||
DenseJointTrajectory = copiedDenseJointTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -130,6 +133,13 @@ public sealed class TrajectoryResult
|
||||
/// </summary>
|
||||
[JsonPropertyName("plannedWaypointCount")]
|
||||
public int PlannedWaypointCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the dense joint trajectory samples where each row is [time, j1, j2, ...].
|
||||
/// Null when dense sampling was not performed (e.g. simulation fallback).
|
||||
/// </summary>
|
||||
[JsonPropertyName("denseJointTrajectory")]
|
||||
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -24,6 +24,50 @@ public sealed class ICspPlanner
|
||||
/// </summary>
|
||||
public const int DefaultMaxIterations = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
|
||||
/// </summary>
|
||||
public const double DefaultFinalScaleTolerance = 1e-2;
|
||||
|
||||
private readonly double _threshold;
|
||||
private readonly int _maxIterations;
|
||||
private readonly bool _enforceFinalScale;
|
||||
private readonly double _finalScaleTolerance;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance)
|
||||
{
|
||||
if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
|
||||
}
|
||||
|
||||
if (maxIterations < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
|
||||
}
|
||||
|
||||
if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
|
||||
}
|
||||
|
||||
_threshold = threshold;
|
||||
_maxIterations = maxIterations;
|
||||
_enforceFinalScale = enforceFinalScale;
|
||||
_finalScaleTolerance = finalScaleTolerance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
@@ -52,7 +96,7 @@ public sealed class ICspPlanner
|
||||
int bestIterations = 0;
|
||||
double[]? bestWaypointTimes = null;
|
||||
|
||||
for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++)
|
||||
for (int iteration = 0; iteration <= _maxIterations; iteration++)
|
||||
{
|
||||
var waypointTimes = CumulativeTimes(segmentDurations);
|
||||
var spline = new CubicSplineInterpolator(waypointTimes, qs);
|
||||
@@ -89,7 +133,7 @@ public sealed class ICspPlanner
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < DefaultThreshold)
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
break;
|
||||
}
|
||||
@@ -105,6 +149,13 @@ public sealed class ICspPlanner
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
|
||||
}
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
|
||||
@@ -24,7 +24,7 @@ public sealed class SelfAdaptIcspPlanner
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner = new();
|
||||
private readonly ICspPlanner _innerPlanner = new(enforceFinalScale: false);
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
|
||||
|
||||
@@ -25,8 +25,9 @@ public sealed class ShotTimelineBuilder
|
||||
/// <param name="trajectory">规划后的轨迹（含补中点信息和机器人配置）。</param>
|
||||
/// <param name="holdCycles">IO 保持周期数（对应原系统的 io_keep_cycles）。</param>
|
||||
/// <param name="samplePeriod">稠密采样周期，用于离散化 sample_index 和 sample_time。</param>
|
||||
/// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
|
||||
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod)
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
@@ -69,6 +70,9 @@ public sealed class ShotTimelineBuilder
|
||||
sampleTime: sampleTime,
|
||||
addressGroup: addressGroup));
|
||||
|
||||
if (useDo)
|
||||
{
|
||||
// use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
@@ -76,6 +80,7 @@ public sealed class ShotTimelineBuilder
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
}
|
||||
}
|
||||
|
||||
return new ShotTimeline(shotEvents, triggerTimeline);
|
||||
}
|
||||
|
||||
@@ -6,4 +6,8 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
116
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
116
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
@@ -0,0 +1,116 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Runtime.Common;
|
||||
|
||||
/// <summary>
|
||||
/// 定义控制器运行时的最小状态与执行契约，供兼容层在不关心底层 Socket 细节的情况下调度轨迹。
|
||||
/// </summary>
|
||||
public interface IControllerRuntime
|
||||
{
|
||||
/// <summary>
|
||||
/// 重置当前机器人模型并清空控制器运行时状态。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="robotName">兼容层传入的机器人名称。</param>
|
||||
void ResetRobot(RobotProfile robot, string robotName);
|
||||
|
||||
/// <summary>
|
||||
/// 选择当前活动控制器类型。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否使用仿真控制器。</param>
|
||||
void SetActiveController(bool sim);
|
||||
|
||||
/// <summary>
|
||||
/// 建立到控制器 IP 的连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 断开当前控制器连接。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 使能机器人并记录底层缓冲区大小。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">运行时缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 关闭机器人使能。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 三维坐标。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>IO 当前值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 写入指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>当前关节位置。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前末端位姿。
|
||||
/// </summary>
|
||||
/// <returns>当前末端位姿。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器状态快照。</returns>
|
||||
ControllerStateSnapshot GetSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 执行一条已经完成规划的轨迹，并更新最终关节位置。
|
||||
/// </summary>
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
|
||||
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
|
||||
}
|
||||
758
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
758
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
@@ -0,0 +1,758 @@
|
||||
using System.Diagnostics;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 控制器运行时，将上层兼容层指令转换为三条真实 Socket 通道的交互。
|
||||
/// 仿真模式下仍保持内存桩行为，便于离线测试与回退。
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly Dictionary<(string IoType, int Port), bool> _ioValues = new();
|
||||
private readonly FanucCommandClient _commandClient;
|
||||
private readonly FanucStateClient _stateClient;
|
||||
private readonly FanucJ519Client _j519Client;
|
||||
|
||||
private RobotProfile? _robot;
|
||||
private string? _robotName;
|
||||
private bool? _activeControllerIsSimulation;
|
||||
private string? _connectedRobotIp;
|
||||
private bool _isEnabled;
|
||||
private bool _isInMotion;
|
||||
private int _bufferSize;
|
||||
private double _speedRatio = 1.0;
|
||||
private double[] _tcp = [0.0, 0.0, 0.0];
|
||||
private double[] _jointPositions = Array.Empty<double>();
|
||||
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
||||
private bool _disposed;
|
||||
private CancellationTokenSource? _sendCts;
|
||||
private Task? _sendTask;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC 控制器运行时。
|
||||
/// </summary>
|
||||
public FanucControllerRuntime()
|
||||
{
|
||||
_commandClient = new FanucCommandClient();
|
||||
_stateClient = new FanucStateClient();
|
||||
_j519Client = new FanucJ519Client();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 供测试注入 mock 客户端的内部构造函数。
|
||||
/// </summary>
|
||||
internal FanucControllerRuntime(FanucCommandClient commandClient, FanucStateClient stateClient, FanucJ519Client j519Client)
|
||||
{
|
||||
_commandClient = commandClient;
|
||||
_stateClient = stateClient;
|
||||
_j519Client = j519Client;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
if (string.IsNullOrWhiteSpace(robotName))
|
||||
{
|
||||
throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
DisconnectClients();
|
||||
_robot = robot;
|
||||
_robotName = robotName;
|
||||
_activeControllerIsSimulation = null;
|
||||
_connectedRobotIp = null;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
_bufferSize = 0;
|
||||
_speedRatio = 1.0;
|
||||
_tcp = [0.0, 0.0, 0.0];
|
||||
_jointPositions = new double[robot.DegreesOfFreedom];
|
||||
_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
||||
_ioValues.Clear();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
DisconnectClients();
|
||||
_activeControllerIsSimulation = sim;
|
||||
_connectedRobotIp = null;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotIp))
|
||||
{
|
||||
throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureActiveControllerSelected();
|
||||
if (_activeControllerIsSimulation!.Value)
|
||||
{
|
||||
_connectedRobotIp = robotIp;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// 真机模式：顺序建立三条通道 — 状态 → 命令 → 运动。
|
||||
_stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult();
|
||||
_commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult();
|
||||
_j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult();
|
||||
|
||||
_connectedRobotIp = robotIp;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
DisconnectClients();
|
||||
_connectedRobotIp = null;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
if (bufferSize <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
_bufferSize = bufferSize;
|
||||
|
||||
if (IsSimulationMode)
|
||||
{
|
||||
_isEnabled = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。
|
||||
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||
_commandClient.ResetRobotAsync().GetAwaiter().GetResult();
|
||||
_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||
_commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||
|
||||
_j519Client.StartMotion();
|
||||
_isEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public double GetSpeedRatio()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
|
||||
_speedRatio = response.Ratio;
|
||||
}
|
||||
|
||||
return _speedRatio;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetSpeedRatio(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_speedRatio = clampedRatio;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetTcp()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp))
|
||||
{
|
||||
var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult();
|
||||
_tcp = response.Pose.Take(3).ToArray();
|
||||
}
|
||||
|
||||
return _tcp.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetTcp(double x, double y, double z)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (_activeControllerIsSimulation is false)
|
||||
{
|
||||
EnsureConnected();
|
||||
_commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_tcp = [x, y, z];
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool GetIo(int port, string ioType)
|
||||
{
|
||||
if (port < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
|
||||
}
|
||||
|
||||
var normalizedIoType = NormalizeIoType(ioType);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
|
||||
_ioValues[(normalizedIoType, port)] = response.Value;
|
||||
return response.Value;
|
||||
}
|
||||
|
||||
return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetIo(int port, bool value, string ioType)
|
||||
{
|
||||
if (port < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(port), "IO 端口不能为负数。");
|
||||
}
|
||||
|
||||
var normalizedIoType = NormalizeIoType(ioType);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureEnabled();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_ioValues[(normalizedIoType, port)] = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetJointPositions()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame?.JointDegrees.Count >= _jointPositions.Length)
|
||||
{
|
||||
return frame.JointDegrees.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
return _jointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame?.CartesianPose.Count >= 6)
|
||||
{
|
||||
return frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
return _pose.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public ControllerStateSnapshot GetSnapshot()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
var jointPositions = _jointPositions;
|
||||
var cartesianPose = _pose;
|
||||
var isInMotion = _isInMotion;
|
||||
IReadOnlyList<uint> stateTailWords = Array.Empty<uint>();
|
||||
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame is not null)
|
||||
{
|
||||
if (frame.JointDegrees.Count >= jointPositions.Length)
|
||||
{
|
||||
jointPositions = frame.JointDegrees.Take(jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
}
|
||||
|
||||
if (frame.CartesianPose.Count >= 6)
|
||||
{
|
||||
cartesianPose = frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
|
||||
}
|
||||
|
||||
stateTailWords = frame.RawTailWords.ToArray();
|
||||
}
|
||||
|
||||
var j519Response = _j519Client.GetLatestResponse();
|
||||
if (j519Response is not null)
|
||||
{
|
||||
isInMotion = j519Response.RobotInMotion;
|
||||
}
|
||||
}
|
||||
|
||||
return new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.UtcNow,
|
||||
connectionState: ResolveConnectionState(),
|
||||
isEnabled: _isEnabled,
|
||||
isInMotion: isInMotion,
|
||||
speedRatio: _speedRatio,
|
||||
jointPositions: jointPositions,
|
||||
cartesianPose: cartesianPose,
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>(),
|
||||
stateTailWords: stateTailWords);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(result);
|
||||
ArgumentNullException.ThrowIfNull(finalJointPositions);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureEnabled();
|
||||
EnsureValidTrajectory(result);
|
||||
EnsureJointCount(finalJointPositions.Count);
|
||||
CancelSendTaskLocked();
|
||||
|
||||
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
|
||||
{
|
||||
// 真机模式且存在稠密路点：启动后台高精度发送任务。
|
||||
_isInMotion = true;
|
||||
_sendCts = new CancellationTokenSource();
|
||||
var ct = _sendCts.Token;
|
||||
_sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
// 真机模式无稠密路点：回退到单点收敛。
|
||||
var command = new FanucJ519Command(
|
||||
sequence: 0,
|
||||
targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
|
||||
_j519Client.UpdateCommand(command);
|
||||
}
|
||||
|
||||
_isInMotion = true;
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放运行时持有的所有 Socket 客户端。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
CancelSendTaskLocked();
|
||||
DisconnectClients();
|
||||
_commandClient.Dispose();
|
||||
_stateClient.Dispose();
|
||||
_j519Client.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台高精度发送任务：按伺服周期遍历稠密路点并注入 IO 触发。
|
||||
/// </summary>
|
||||
private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions, CancellationToken cancellationToken)
|
||||
{
|
||||
var denseTrajectory = result.DenseJointTrajectory!;
|
||||
var triggers = result.TriggerTimeline;
|
||||
var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds;
|
||||
var halfServoPeriod = servoPeriodSeconds / 2.0;
|
||||
var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
long nextTick = stopwatch.ElapsedTicks;
|
||||
uint sequence = 0;
|
||||
ushort ioValue = 0;
|
||||
int holdRemaining = -1;
|
||||
|
||||
try
|
||||
{
|
||||
foreach (var row in denseTrajectory)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
nextTick += periodTicks;
|
||||
|
||||
double t = row[0];
|
||||
var joints = row.Skip(1).Select(static v => (double)v).ToArray();
|
||||
|
||||
// 递减 IO 保持计数器；若已到期则清零。
|
||||
if (holdRemaining > 0)
|
||||
{
|
||||
holdRemaining--;
|
||||
}
|
||||
else if (holdRemaining == 0)
|
||||
{
|
||||
ioValue = 0;
|
||||
holdRemaining = -1;
|
||||
}
|
||||
|
||||
// 检查当前周期是否有新的触发事件。
|
||||
if (holdRemaining < 0)
|
||||
{
|
||||
foreach (var trigger in triggers)
|
||||
{
|
||||
if (Math.Abs(t - trigger.TriggerTime) < halfServoPeriod)
|
||||
{
|
||||
ioValue = ComputeIoValue(trigger.AddressGroup);
|
||||
holdRemaining = trigger.HoldCycles;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var command = new FanucJ519Command(
|
||||
sequence: sequence++,
|
||||
targetJoints: joints,
|
||||
writeIoType: 2,
|
||||
writeIoIndex: 1,
|
||||
writeIoMask: 255,
|
||||
writeIoValue: ioValue);
|
||||
|
||||
_j519Client.UpdateCommand(command);
|
||||
|
||||
// 高精度忙等待直到下一伺服周期。
|
||||
while (stopwatch.ElapsedTicks < nextTick)
|
||||
{
|
||||
Thread.SpinWait(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消，轨迹被中断。
|
||||
}
|
||||
finally
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。
|
||||
/// </summary>
|
||||
private void CancelSendTaskLocked()
|
||||
{
|
||||
_sendCts?.Cancel();
|
||||
|
||||
if (_sendTask is not null)
|
||||
{
|
||||
try
|
||||
{
|
||||
_sendTask.Wait(TimeSpan.FromSeconds(2));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_sendTask = null;
|
||||
}
|
||||
|
||||
_sendCts?.Dispose();
|
||||
_sendCts = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码。
|
||||
/// </summary>
|
||||
internal static ushort ComputeIoValue(IoAddressGroup group)
|
||||
{
|
||||
ushort value = 0;
|
||||
foreach (var addr in group.Addresses)
|
||||
{
|
||||
if (addr is >= 0 and < 16)
|
||||
{
|
||||
value |= (ushort)(1 << addr);
|
||||
}
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断当前是否处于仿真模式；若尚未选择控制器则抛出异常。
|
||||
/// </summary>
|
||||
private bool IsSimulationMode
|
||||
{
|
||||
get
|
||||
{
|
||||
if (_activeControllerIsSimulation is null)
|
||||
{
|
||||
throw new InvalidOperationException("Active controller has not been selected.");
|
||||
}
|
||||
|
||||
return _activeControllerIsSimulation.Value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开所有真实 Socket 通道，不影响内存状态。
|
||||
/// </summary>
|
||||
private void DisconnectClients()
|
||||
{
|
||||
_j519Client.Disconnect();
|
||||
_commandClient.Disconnect();
|
||||
_stateClient.Disconnect();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 归一化 IO 类型字符串，避免调用方大小写差异影响缓存键。
|
||||
/// </summary>
|
||||
private static string NormalizeIoType(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP 层三维 TCP 请求扩展为 FANUC 命令通道需要的 7 维 Pose。
|
||||
/// </summary>
|
||||
private static double[] CreateTcpPose(double x, double y, double z)
|
||||
{
|
||||
return [x, y, z, 0.0, 0.0, 0.0, 1.0];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验轨迹规划结果可执行。
|
||||
/// </summary>
|
||||
private static void EnsureValidTrajectory(TrajectoryResult result)
|
||||
{
|
||||
if (!result.IsValid)
|
||||
{
|
||||
throw new InvalidOperationException(result.FailureReason ?? "Trajectory result is invalid.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据当前内部状态生成连接状态标签。
|
||||
/// </summary>
|
||||
private string ResolveConnectionState()
|
||||
{
|
||||
if (_robot is null)
|
||||
{
|
||||
return "NotConfigured";
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_connectedRobotIp))
|
||||
{
|
||||
return "Disconnected";
|
||||
}
|
||||
|
||||
return _activeControllerIsSimulation is false
|
||||
? ResolveRealConnectionState(_stateClient.GetStatus())
|
||||
: "Connected";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把真实 10010 状态通道健康度映射为上层快照连接状态。
|
||||
/// </summary>
|
||||
internal static string ResolveRealConnectionState(FanucStateClientStatus status)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(status);
|
||||
|
||||
return status.State switch
|
||||
{
|
||||
FanucStateConnectionState.Connected when status.IsFrameStale => "StateTimeout",
|
||||
FanucStateConnectionState.Connected => "Connected",
|
||||
FanucStateConnectionState.TimedOut => "StateTimeout",
|
||||
FanucStateConnectionState.Reconnecting => "Reconnecting",
|
||||
FanucStateConnectionState.Connecting => "Connecting",
|
||||
_ => "Disconnected",
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断 runtime 是否可以把某个状态通道帧作为当前机器人状态使用。
|
||||
/// </summary>
|
||||
internal static bool ShouldUseStateFrame(FanucStateClientStatus status)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(status);
|
||||
return status.State == FanucStateConnectionState.Connected && !status.IsFrameStale;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取未超时的状态帧；超时或重连期间不把旧状态作为当前机器人状态使用。
|
||||
/// </summary>
|
||||
private FanucStateFrame? GetFreshStateFrame()
|
||||
{
|
||||
var status = _stateClient.GetStatus();
|
||||
return ShouldUseStateFrame(status) ? _stateClient.GetLatestFrame() : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验给定关节数组长度与当前机器人自由度一致。
|
||||
/// </summary>
|
||||
private void EnsureJointCount(int jointCount)
|
||||
{
|
||||
var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
if (jointCount != expectedJointCount)
|
||||
{
|
||||
throw new InvalidOperationException($"Expected {expectedJointCount} joints but received {jointCount}.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验机器人已经完成初始化。
|
||||
/// </summary>
|
||||
private void EnsureRobotSetup()
|
||||
{
|
||||
if (_robot is null || string.IsNullOrWhiteSpace(_robotName))
|
||||
{
|
||||
throw new InvalidOperationException("Robot has not been setup.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验活动控制器已经被选择。
|
||||
/// </summary>
|
||||
private void EnsureActiveControllerSelected()
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (_activeControllerIsSimulation is null)
|
||||
{
|
||||
throw new InvalidOperationException("Active controller has not been selected.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验控制器已经建立连接。
|
||||
/// </summary>
|
||||
private void EnsureConnected()
|
||||
{
|
||||
EnsureActiveControllerSelected();
|
||||
if (string.IsNullOrWhiteSpace(_connectedRobotIp))
|
||||
{
|
||||
throw new InvalidOperationException("Controller has not been connected.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验机器人已经处于使能态。
|
||||
/// </summary>
|
||||
private void EnsureEnabled()
|
||||
{
|
||||
EnsureConnected();
|
||||
if (!_isEnabled || _bufferSize <= 0)
|
||||
{
|
||||
throw new InvalidOperationException("Robot has not been enabled.");
|
||||
}
|
||||
}
|
||||
}
|
||||
20
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
20
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
@@ -0,0 +1,20 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
405
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
405
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
@@ -0,0 +1,405 @@
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10012 命令通道客户端，提供 Req/Res 同步命令下发能力。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandClient : IDisposable
|
||||
{
|
||||
private readonly SemaphoreSlim _sendLock = new(1, 1);
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">命令通道端口，默认 10012。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_tcpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_tcpClient = new TcpClient { NoDelay = true };
|
||||
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_stream = _tcpClient.GetStream();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开命令通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送通用命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">命令业务体。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>原始响应帧。</returns>
|
||||
public async Task<byte[]> SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackFrame(messageId, body.Span);
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送携带程序名的命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
return SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定程序状态。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
_sendLock.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 直接发送已封装的帧并读取响应。
|
||||
/// </summary>
|
||||
private async Task<byte[]> SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验普通命令响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验程序状态响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验速度倍率响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 TCP 位姿响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 IO 读取响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucIoResponse EnsureSuccess(FanucIoResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造包含 FANUC 命令上下文的失败异常。
|
||||
/// </summary>
|
||||
private static InvalidOperationException CreateCommandFailureException(uint messageId, uint resultCode)
|
||||
{
|
||||
return new InvalidOperationException(
|
||||
$"FANUC command 0x{messageId:X4} failed with result_code {resultCode}.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中读取一条完整的 doz/zod 响应帧。
|
||||
/// </summary>
|
||||
private async Task<byte[]> ReadResponseFrameAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
// 先读取 11 字节头：doz(3) + length(4) + msg_id(4)
|
||||
var header = new byte[11];
|
||||
await ReadExactAsync(header, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
if (header[0] != (byte)'d' || header[1] != (byte)'o' || header[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("响应帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(header.AsSpan(3, 4));
|
||||
if (declaredLength < 14)
|
||||
{
|
||||
throw new InvalidDataException("响应帧声明长度过短。");
|
||||
}
|
||||
|
||||
var remaining = (int)declaredLength - 11;
|
||||
var frame = new byte[declaredLength];
|
||||
header.CopyTo(frame, 0);
|
||||
await ReadExactAsync(frame.AsMemory(11, remaining), cancellationToken).ConfigureAwait(false);
|
||||
|
||||
// 校验帧尾
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("响应帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取指定长度的字节。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(Memory<byte> buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
var totalRead = 0;
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await _stream.ReadAsync(buffer.Slice(totalRead), cancellationToken).ConfigureAwait(false);
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("命令通道已断开，读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
}
|
||||
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
@@ -0,0 +1,621 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
|
||||
/// </summary>
|
||||
public static class FanucCommandMessageIds
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态的消息号。
|
||||
/// </summary>
|
||||
public const uint GetProgramStatus = 0x2003;
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器的消息号。
|
||||
/// </summary>
|
||||
public const uint ResetRobot = 0x2100;
|
||||
|
||||
/// <summary>
|
||||
/// 启动控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StartProgram = 0x2102;
|
||||
|
||||
/// <summary>
|
||||
/// 停止控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StopProgram = 0x2103;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetTcp = 0x2200;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetTcp = 0x2201;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint GetSpeedRatio = 0x2206;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint SetSpeedRatio = 0x2207;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetIo = 0x2208;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetIo = 0x2209;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||||
/// </summary>
|
||||
public static class FanucIoTypes
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 数字输入 DI。
|
||||
/// </summary>
|
||||
public const uint DigitalInput = 1;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 数字输出 DO。
|
||||
/// </summary>
|
||||
public const uint DigitalOutput = 2;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输入 RI。
|
||||
/// </summary>
|
||||
public const uint RobotInput = 8;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输出 RO。
|
||||
/// </summary>
|
||||
public const uint RobotOutput = 9;
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
|
||||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||||
public static uint FromName(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant() switch
|
||||
{
|
||||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||||
"RI" or "KIOTYPERI" => RobotInput,
|
||||
"RO" or "KIOTYPERO" => RobotOutput,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandResultResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化命令结果响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucCommandResultResponse(uint messageId, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||||
/// </summary>
|
||||
public sealed class FanucSpeedRatioResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化速度倍率响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
RatioInt = ratioInt;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的整数百分比。
|
||||
/// </summary>
|
||||
public uint RatioInt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||||
/// </summary>
|
||||
public double Ratio => RatioInt / 100.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||||
/// </summary>
|
||||
public sealed class FanucTcpResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 TCP 位姿响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
TcpId = tcpId;
|
||||
Pose = pose.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的 TCP ID。
|
||||
/// </summary>
|
||||
public uint TcpId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 7 维 TCP 位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||||
/// </summary>
|
||||
public sealed class FanucIoResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 IO 读取响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
NumericValue = numericValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的原始数值。
|
||||
/// </summary>
|
||||
public double NumericValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取按布尔 IO 解释后的值。
|
||||
/// </summary>
|
||||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||
/// </summary>
|
||||
public sealed class FanucProgramStatusResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化程序状态响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="programStatus">控制器程序状态。</param>
|
||||
public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
ProgramStatus = programStatus;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态值。
|
||||
/// </summary>
|
||||
public uint ProgramStatus { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucCommandProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// 将无业务体命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackEmptyCommand(uint messageId)
|
||||
{
|
||||
return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将程序名命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">控制器程序名。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackProgramCommand(uint messageId, string programName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
throw new ArgumentException("程序名不能为空。", nameof(programName));
|
||||
}
|
||||
|
||||
var programNameBytes = Encoding.ASCII.GetBytes(programName);
|
||||
var body = new byte[sizeof(uint) + programNameBytes.Length];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
|
||||
programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
|
||||
|
||||
return PackFrame(messageId, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取速度倍率命令。
|
||||
/// </summary>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetSpeedRatioCommand()
|
||||
{
|
||||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||||
{
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||||
}
|
||||
|
||||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 布尔值。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>命令结果响应。</returns>
|
||||
public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint))
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令响应体长度不足。");
|
||||
}
|
||||
|
||||
return new FanucCommandResultResponse(
|
||||
messageId,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||||
}
|
||||
|
||||
// GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
|
||||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||||
{
|
||||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
var pose = new double[7];
|
||||
for (int i = 0; i < pose.Length; i++)
|
||||
{
|
||||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||||
}
|
||||
|
||||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) + sizeof(float))
|
||||
{
|
||||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||||
}
|
||||
|
||||
// 抓包样本中的字段顺序为 result_code 后接 prog_status。
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按 FANUC 命令通道 framing 规则封装完整帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">业务体。</param>
|
||||
/// <returns>完整命令帧。</returns>
|
||||
internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
|
||||
{
|
||||
var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
|
||||
var frame = new byte[frameLength];
|
||||
|
||||
frame[0] = (byte)'d';
|
||||
frame[1] = (byte)'o';
|
||||
frame[2] = (byte)'z';
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
|
||||
body.CopyTo(frame.AsSpan(11));
|
||||
frame[^3] = (byte)'z';
|
||||
frame[^2] = (byte)'o';
|
||||
frame[^1] = (byte)'d';
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验完整帧并读取消息号。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>响应消息号。</returns>
|
||||
private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length < 14)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度不足。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
|
||||
}
|
||||
|
||||
return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取完整帧中的业务体切片。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>业务体切片。</returns>
|
||||
private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
return frame.Slice(11, frame.Length - 14);
|
||||
}
|
||||
}
|
||||
322
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
322
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
@@ -0,0 +1,322 @@
|
||||
using System.Diagnostics;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供周期命令发送与响应接收能力。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Client : IDisposable
|
||||
{
|
||||
private readonly object _commandLock = new();
|
||||
private readonly object _responseLock = new();
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _cts;
|
||||
private Task? _sendTask;
|
||||
private Task? _receiveTask;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已创建 UDP 套接字。
|
||||
/// </summary>
|
||||
public bool IsConnected => _udpClient is not null;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">运动通道端口，默认 60015。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_udpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_udpClient = new UdpClient();
|
||||
_udpClient.Connect(ip, port);
|
||||
|
||||
// 发送初始化包。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
_receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动约 8ms 周期的 J519 命令发送循环。
|
||||
/// </summary>
|
||||
public void StartMotion()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道未连接。");
|
||||
}
|
||||
|
||||
if (_sendTask is not null)
|
||||
{
|
||||
return; // 已在运行。
|
||||
}
|
||||
|
||||
_sendTask = Task.Run(() => SendLoopAsync(_cts!.Token), _cts!.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送结束包并停止 J519 命令发送循环。
|
||||
/// </summary>
|
||||
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_cts?.Cancel();
|
||||
|
||||
if (_sendTask is not null)
|
||||
{
|
||||
try
|
||||
{
|
||||
await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
catch (TimeoutException)
|
||||
{
|
||||
// 发送循环可能未能在 1 秒内结束，继续执行后续清理。
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消。
|
||||
}
|
||||
|
||||
_sendTask = null;
|
||||
}
|
||||
|
||||
// 发送结束包通知控制器停止伺服流。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 原子更新下一周期要发送的 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="command">新的 J519 命令。</param>
|
||||
public void UpdateCommand(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = command;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次通过 UpdateCommand 设置的 J519 命令；供测试断言使用。
|
||||
/// </summary>
|
||||
/// <returns>当前 J519 命令或 null。</returns>
|
||||
internal FanucJ519Command? GetCurrentCommand()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _currentCommand;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新 J519 响应或 null。</returns>
|
||||
public FanucJ519Response? GetLatestResponse()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
return _latestResponse;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开 J519 通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_sendTask?.Dispose();
|
||||
_sendTask = null;
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_receiveTask?.Dispose();
|
||||
_receiveTask = null;
|
||||
|
||||
_cts?.Dispose();
|
||||
_cts = null;
|
||||
|
||||
_udpClient?.Dispose();
|
||||
_udpClient = null;
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_sendTask?.Dispose();
|
||||
_receiveTask?.Dispose();
|
||||
_cts?.Dispose();
|
||||
_udpClient?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台发送循环：以 Stopwatch + SpinWait 实现高精度 8ms 周期发送当前命令。
|
||||
/// </summary>
|
||||
private async Task SendLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// 8ms 伺服周期，对应 125Hz。
|
||||
var periodTicks = (long)(0.008 * Stopwatch.Frequency);
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
long nextTick = stopwatch.ElapsedTicks;
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
nextTick += periodTicks;
|
||||
|
||||
FanucJ519Command? command;
|
||||
lock (_commandLock)
|
||||
{
|
||||
command = _currentCommand;
|
||||
}
|
||||
|
||||
if (command is not null)
|
||||
{
|
||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||
await _udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
|
||||
// 高精度忙等待直到下一周期，避免 PeriodicTimer 的 ±15ms 抖动。
|
||||
while (stopwatch.ElapsedTicks < nextTick)
|
||||
{
|
||||
Thread.SpinWait(1);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消，退出循环。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台接收循环：持续接收 132B 响应并解析。
|
||||
/// </summary>
|
||||
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false);
|
||||
if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength)
|
||||
{
|
||||
var response = FanucJ519Protocol.ParseResponse(result.Buffer);
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = response;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消，退出循环。
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// UDP 客户端已释放，退出循环。
|
||||
}
|
||||
}
|
||||
}
|
||||
386
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
386
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
@@ -0,0 +1,386 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Command
|
||||
{
|
||||
private readonly double[] _targetJoints;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 命令数据。
|
||||
/// </summary>
|
||||
/// <param name="sequence">命令序号。</param>
|
||||
/// <param name="targetJoints">目标关节或扩展轴数据，最多 9 个槽位。</param>
|
||||
/// <param name="lastData">是否为最后一帧数据。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="dataStyle">目标数据类型。</param>
|
||||
/// <param name="writeIoType">写入 IO 类型。</param>
|
||||
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
|
||||
/// <param name="writeIoMask">写入 IO 掩码。</param>
|
||||
/// <param name="writeIoValue">写入 IO 数值。</param>
|
||||
public FanucJ519Command(
|
||||
uint sequence,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
byte lastData = 0,
|
||||
byte readIoType = 2,
|
||||
ushort readIoIndex = 1,
|
||||
ushort readIoMask = 255,
|
||||
byte dataStyle = 1,
|
||||
byte writeIoType = 2,
|
||||
ushort writeIoIndex = 1,
|
||||
ushort writeIoMask = 0,
|
||||
ushort writeIoValue = 0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
if (targetJoints.Count is <= 0 or > 9)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
|
||||
}
|
||||
|
||||
Sequence = sequence;
|
||||
LastData = lastData;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
DataStyle = dataStyle;
|
||||
WriteIoType = writeIoType;
|
||||
WriteIoIndex = writeIoIndex;
|
||||
WriteIoMask = writeIoMask;
|
||||
WriteIoValue = writeIoValue;
|
||||
_targetJoints = targetJoints.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取命令序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否为最后一帧数据。
|
||||
/// </summary>
|
||||
public byte LastData { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标数据类型。
|
||||
/// </summary>
|
||||
public byte DataStyle { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 类型。
|
||||
/// </summary>
|
||||
public byte WriteIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort WriteIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort WriteIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 数值。
|
||||
/// </summary>
|
||||
public ushort WriteIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标关节或扩展轴数据。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> TargetJoints => _targetJoints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Response
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly double[] _jointDegrees;
|
||||
private readonly double[] _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 响应数据。
|
||||
/// </summary>
|
||||
/// <param name="messageType">响应类型。</param>
|
||||
/// <param name="version">协议版本。</param>
|
||||
/// <param name="sequence">响应序号。</param>
|
||||
/// <param name="status">状态位集合。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="readIoValue">读取 IO 数值。</param>
|
||||
/// <param name="timestamp">控制器时间戳。</param>
|
||||
/// <param name="pose">TCP 笛卡尔位姿。</param>
|
||||
/// <param name="externalAxes">扩展轴反馈。</param>
|
||||
/// <param name="jointDegrees">关节角度反馈。</param>
|
||||
/// <param name="motorCurrents">电机电流反馈。</param>
|
||||
public FanucJ519Response(
|
||||
uint messageType,
|
||||
uint version,
|
||||
uint sequence,
|
||||
byte status,
|
||||
byte readIoType,
|
||||
ushort readIoIndex,
|
||||
ushort readIoMask,
|
||||
ushort readIoValue,
|
||||
uint timestamp,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> externalAxes,
|
||||
IEnumerable<double> jointDegrees,
|
||||
IEnumerable<double> motorCurrents)
|
||||
{
|
||||
MessageType = messageType;
|
||||
Version = version;
|
||||
Sequence = sequence;
|
||||
Status = status;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
ReadIoValue = readIoValue;
|
||||
Timestamp = timestamp;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
|
||||
_jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
|
||||
_motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应类型。
|
||||
/// </summary>
|
||||
public uint MessageType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取协议版本。
|
||||
/// </summary>
|
||||
public uint Version { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态位集合。
|
||||
/// </summary>
|
||||
public byte Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 数值。
|
||||
/// </summary>
|
||||
public ushort ReadIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器时间戳。
|
||||
/// </summary>
|
||||
public uint Timestamp { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取扩展轴反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取关节角度反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 获取电机电流反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> MotorCurrents => _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否接受命令。
|
||||
/// </summary>
|
||||
public bool AcceptsCommand => (Status & 0b0001) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否已收到命令。
|
||||
/// </summary>
|
||||
public bool ReceivedCommand => (Status & 0b0010) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器系统是否就绪。
|
||||
/// </summary>
|
||||
public bool SystemReady => (Status & 0b0100) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取机器人是否处于运动中。
|
||||
/// </summary>
|
||||
public bool RobotInMotion => (Status & 0b1000) != 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucJ519Protocol
|
||||
{
|
||||
/// <summary>
|
||||
/// J519 初始化和结束控制包长度。
|
||||
/// </summary>
|
||||
public const int ControlPacketLength = 8;
|
||||
|
||||
/// <summary>
|
||||
/// J519 命令包长度。
|
||||
/// </summary>
|
||||
public const int CommandPacketLength = 64;
|
||||
|
||||
/// <summary>
|
||||
/// J519 响应包长度。
|
||||
/// </summary>
|
||||
public const int ResponsePacketLength = 132;
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 初始化包。
|
||||
/// </summary>
|
||||
/// <returns>初始化包。</returns>
|
||||
public static byte[] PackInitPacket()
|
||||
{
|
||||
return PackControlPacket(0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 结束包。
|
||||
/// </summary>
|
||||
/// <returns>结束包。</returns>
|
||||
public static byte[] PackEndPacket()
|
||||
{
|
||||
return PackControlPacket(2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 64 字节命令包。
|
||||
/// </summary>
|
||||
/// <param name="command">命令数据。</param>
|
||||
/// <returns>命令包。</returns>
|
||||
public static byte[] PackCommandPacket(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
|
||||
var packet = new byte[CommandPacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
|
||||
packet[0x0c] = command.LastData;
|
||||
packet[0x0d] = command.ReadIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
|
||||
packet[0x12] = command.DataStyle;
|
||||
packet[0x13] = command.WriteIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);
|
||||
|
||||
// J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
|
||||
for (var index = 0; index < 9; index++)
|
||||
{
|
||||
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
||||
}
|
||||
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 J519 132 字节响应包。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <returns>响应解析结果。</returns>
|
||||
public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
|
||||
{
|
||||
if (packet.Length != ResponsePacketLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC J519 响应包长度不正确。");
|
||||
}
|
||||
|
||||
return new FanucJ519Response(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
|
||||
packet[0x0c],
|
||||
packet[0x0d],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
|
||||
ReadFloatArray(packet, 0x18, 6),
|
||||
ReadFloatArray(packet, 0x30, 3),
|
||||
ReadFloatArray(packet, 0x3c, 9),
|
||||
ReadFloatArray(packet, 0x60, 9));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 控制包。
|
||||
/// </summary>
|
||||
/// <param name="packetType">控制包类型。</param>
|
||||
/// <returns>控制包。</returns>
|
||||
private static byte[] PackControlPacket(uint packetType)
|
||||
{
|
||||
var packet = new byte[ControlPacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从响应包中读取固定长度 f32 数组。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <param name="offset">数组起始偏移。</param>
|
||||
/// <param name="count">数组元素数量。</param>
|
||||
/// <returns>转换成 double 的数值数组。</returns>
|
||||
private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
|
||||
{
|
||||
var values = new double[count];
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
return values;
|
||||
}
|
||||
}
|
||||
561
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
561
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
@@ -0,0 +1,561 @@
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的连接阶段。
|
||||
/// </summary>
|
||||
public enum FanucStateConnectionState
|
||||
{
|
||||
/// <summary>
|
||||
/// 状态通道未连接。
|
||||
/// </summary>
|
||||
Disconnected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在建立连接。
|
||||
/// </summary>
|
||||
Connecting,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道已连接并由后台循环接收状态帧。
|
||||
/// </summary>
|
||||
Connected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道在限定时间内没有收到完整状态帧。
|
||||
/// </summary>
|
||||
TimedOut,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在按退避策略重新连接。
|
||||
/// </summary>
|
||||
Reconnecting,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10010 状态通道的超时和重连参数。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置接收一帧完整 90B 状态帧允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan FrameTimeout { get; init; } = TimeSpan.FromMilliseconds(250);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置初始重连等待时间。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectInitialDelay { get; init; } = TimeSpan.FromMilliseconds(100);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置重连等待时间的上限。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置单次 TCP 建连允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的当前诊断状态。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientStatus
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化状态通道诊断状态。
|
||||
/// </summary>
|
||||
public FanucStateClientStatus(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
DateTimeOffset? lastFrameAt,
|
||||
long reconnectAttemptCount,
|
||||
string? lastErrorMessage)
|
||||
{
|
||||
State = state;
|
||||
IsFrameStale = isFrameStale;
|
||||
LastFrameAt = lastFrameAt;
|
||||
ReconnectAttemptCount = reconnectAttemptCount;
|
||||
LastErrorMessage = lastErrorMessage;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前连接阶段。
|
||||
/// </summary>
|
||||
public FanucStateConnectionState State { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近缓存状态帧是否已经超过状态帧超时窗口。
|
||||
/// </summary>
|
||||
public bool IsFrameStale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次成功解析状态帧的 UTC 时间。
|
||||
/// </summary>
|
||||
public DateTimeOffset? LastFrameAt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取后台循环发起重连的累计次数。
|
||||
/// </summary>
|
||||
public long ReconnectAttemptCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次状态通道异常的诊断文本。
|
||||
/// </summary>
|
||||
public string? LastErrorMessage { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10010 状态通道客户端，持续接收状态帧并缓存最新快照。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClient : IDisposable
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly FanucStateClientOptions _options;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private Task? _receiveTask;
|
||||
private FanucStateFrame? _latestFrame;
|
||||
private FanucStateConnectionState _connectionState = FanucStateConnectionState.Disconnected;
|
||||
private DateTimeOffset? _lastConnectedAt;
|
||||
private DateTimeOffset? _lastFrameAt;
|
||||
private long _reconnectAttemptCount;
|
||||
private string? _lastErrorMessage;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 使用默认状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
public FanucStateClient()
|
||||
: this(new FanucStateClientOptions())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(options);
|
||||
ValidateOptions(options);
|
||||
_options = options;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => GetStatus().State == FanucStateConnectionState.Connected;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">状态通道端口，默认 10010。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10010, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_receiveTask is not null)
|
||||
{
|
||||
throw new InvalidOperationException("状态通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_receiveCts = new CancellationTokenSource();
|
||||
using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _receiveCts.Token);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Connecting;
|
||||
_latestFrame = null;
|
||||
_lastConnectedAt = null;
|
||||
_lastFrameAt = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, linkedCts.Token).ConfigureAwait(false);
|
||||
}
|
||||
catch
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
}
|
||||
|
||||
_receiveCts.Dispose();
|
||||
_receiveCts = null;
|
||||
throw;
|
||||
}
|
||||
|
||||
_receiveTask = Task.Run(
|
||||
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
|
||||
_receiveCts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开状态通道并停止后台接收循环。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的状态帧；若尚未收到任何帧则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新状态帧或 null。</returns>
|
||||
public FanucStateFrame? GetLatestFrame()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _latestFrame;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前诊断状态。
|
||||
/// </summary>
|
||||
/// <returns>状态通道诊断快照。</returns>
|
||||
public FanucStateClientStatus GetStatus()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return new FanucStateClientStatus(
|
||||
_connectionState,
|
||||
IsFrameStaleLocked(DateTimeOffset.UtcNow),
|
||||
_lastFrameAt,
|
||||
_reconnectAttemptCount,
|
||||
_lastErrorMessage);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台循环：持续接收状态帧；断线、超时或坏帧后进入退避重连。
|
||||
/// </summary>
|
||||
private async Task ReceiveAndReconnectLoopAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var reconnectDelay = _options.ReconnectInitialDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
|
||||
reconnectDelay = _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
catch (TimeoutException ex)
|
||||
{
|
||||
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
|
||||
}
|
||||
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
|
||||
{
|
||||
MarkReceiveFailure(FanucStateConnectionState.Reconnecting, ex.Message);
|
||||
}
|
||||
|
||||
CloseCurrentConnection();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
reconnectDelay = await ReconnectWithBackoffAsync(ip, port, reconnectDelay, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前连接中持续读取状态帧，直到连接异常或被取消。
|
||||
/// </summary>
|
||||
private async Task ReceiveCurrentConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
NetworkStream stream;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream ?? throw new IOException("状态通道未连接。");
|
||||
}
|
||||
|
||||
var buffer = new byte[FanucStateProtocol.StateFrameLength];
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await ReadExactAsync(stream, buffer, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(buffer);
|
||||
lock (_stateLock)
|
||||
{
|
||||
_latestFrame = frame;
|
||||
_lastFrameAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取固定长度字节，超过帧超时窗口则抛出超时异常。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.FrameTimeout);
|
||||
|
||||
var totalRead = 0;
|
||||
try
|
||||
{
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await stream.ReadAsync(
|
||||
buffer.AsMemory(totalRead, buffer.Length - totalRead),
|
||||
timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("状态通道已断开，读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw new TimeoutException("状态通道接收超时，未在限定时间内收到完整 90B 状态帧。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开 TCP 状态通道并更新连接状态。
|
||||
/// </summary>
|
||||
private async Task OpenConnectionAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var tcpClient = new TcpClient { NoDelay = true };
|
||||
try
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.ConnectTimeout);
|
||||
await tcpClient.ConnectAsync(ip, port, timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_tcpClient = tcpClient;
|
||||
_stream = tcpClient.GetStream();
|
||||
_lastConnectedAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
tcpClient.Dispose();
|
||||
throw new TimeoutException("状态通道建连超时。");
|
||||
}
|
||||
catch
|
||||
{
|
||||
tcpClient.Dispose();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按退避策略循环尝试重新连接，并返回下一次异常后的退避时间。
|
||||
/// </summary>
|
||||
private async Task<TimeSpan> ReconnectWithBackoffAsync(
|
||||
string ip,
|
||||
int port,
|
||||
TimeSpan reconnectDelay,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var nextDelay = reconnectDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
}
|
||||
|
||||
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_reconnectAttemptCount++;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
return _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex) when (ex is SocketException or IOException or TimeoutException)
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
_lastErrorMessage = ex.Message;
|
||||
}
|
||||
|
||||
nextDelay = IncreaseReconnectDelay(nextDelay);
|
||||
}
|
||||
}
|
||||
|
||||
return nextDelay;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭当前 TCP 连接，不清除最新状态帧，供重连路径保留诊断数据。
|
||||
/// </summary>
|
||||
private void CloseCurrentConnection()
|
||||
{
|
||||
NetworkStream? stream;
|
||||
TcpClient? tcpClient;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream;
|
||||
tcpClient = _tcpClient;
|
||||
_stream = null;
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
stream?.Dispose();
|
||||
tcpClient?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录接收异常并更新状态通道连接阶段。
|
||||
/// </summary>
|
||||
private void MarkReceiveFailure(FanucStateConnectionState state, string message)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = state;
|
||||
_lastErrorMessage = message;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭后台循环和 socket 资源。
|
||||
/// </summary>
|
||||
private void Shutdown(bool clearLatestFrame)
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
CloseCurrentConnection();
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(2));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 后台循环可能因取消而抛出 OperationCanceledException，忽略即可。
|
||||
}
|
||||
|
||||
_receiveTask = null;
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
_lastConnectedAt = null;
|
||||
_lastErrorMessage = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
if (clearLatestFrame)
|
||||
{
|
||||
_latestFrame = null;
|
||||
_lastFrameAt = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断缓存帧是否已经不能代表当前控制柜状态。
|
||||
/// </summary>
|
||||
private bool IsFrameStaleLocked(DateTimeOffset now)
|
||||
{
|
||||
if (_latestFrame is null)
|
||||
{
|
||||
return _connectionState is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting
|
||||
|| _reconnectAttemptCount > 0
|
||||
|| (_lastConnectedAt.HasValue && now - _lastConnectedAt.Value > _options.FrameTimeout);
|
||||
}
|
||||
|
||||
return _lastFrameAt.HasValue && now - _lastFrameAt.Value > _options.FrameTimeout;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算下一轮重连等待时间。
|
||||
/// </summary>
|
||||
private TimeSpan IncreaseReconnectDelay(TimeSpan currentDelay)
|
||||
{
|
||||
var doubledMilliseconds = Math.Max(currentDelay.TotalMilliseconds * 2.0, _options.ReconnectInitialDelay.TotalMilliseconds);
|
||||
var cappedMilliseconds = Math.Min(doubledMilliseconds, _options.ReconnectMaxDelay.TotalMilliseconds);
|
||||
return TimeSpan.FromMilliseconds(cappedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态通道参数，避免后台循环使用无效时间窗口。
|
||||
/// </summary>
|
||||
private static void ValidateOptions(FanucStateClientOptions options)
|
||||
{
|
||||
ValidatePositive(options.FrameTimeout, nameof(options.FrameTimeout));
|
||||
ValidatePositive(options.ReconnectInitialDelay, nameof(options.ReconnectInitialDelay));
|
||||
ValidatePositive(options.ReconnectMaxDelay, nameof(options.ReconnectMaxDelay));
|
||||
ValidatePositive(options.ConnectTimeout, nameof(options.ConnectTimeout));
|
||||
|
||||
if (options.ReconnectMaxDelay < options.ReconnectInitialDelay)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(options), "最大重连等待时间不能小于初始重连等待时间。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验时间参数必须为正值。
|
||||
/// </summary>
|
||||
private static void ValidatePositive(TimeSpan value, string parameterName)
|
||||
{
|
||||
if (value <= TimeSpan.Zero)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(parameterName, "时间参数必须大于 0。");
|
||||
}
|
||||
}
|
||||
}
|
||||
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
@@ -0,0 +1,187 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
|
||||
/// </summary>
|
||||
public sealed class FanucStateFrame
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _jointOrExtensionValues;
|
||||
private readonly double[] _jointDegrees;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly uint[] _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态帧解析结果。
|
||||
/// </summary>
|
||||
/// <param name="messageId">状态帧消息号或序号。</param>
|
||||
/// <param name="pose">控制器回传的笛卡尔位姿。</param>
|
||||
/// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
|
||||
/// <param name="tailWords">状态帧尾部状态槽位。</param>
|
||||
public FanucStateFrame(
|
||||
uint messageId,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> jointOrExtensionValues,
|
||||
IEnumerable<uint> tailWords)
|
||||
{
|
||||
MessageId = messageId;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
|
||||
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
|
||||
|
||||
if (_pose.Length != 6)
|
||||
{
|
||||
throw new ArgumentException("状态帧位姿必须包含 6 个 float。", nameof(pose));
|
||||
}
|
||||
|
||||
if (_jointOrExtensionValues.Length != 9)
|
||||
{
|
||||
throw new ArgumentException("状态帧关节/扩展轴必须包含 9 个 float。", nameof(jointOrExtensionValues));
|
||||
}
|
||||
|
||||
if (_tailWords.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
||||
}
|
||||
|
||||
_jointDegrees = _jointOrExtensionValues.Take(6).ToArray();
|
||||
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧消息号或序号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿 X/Y/Z/W/P/R，单位来自 FANUC 状态服务器。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> CartesianPose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的关节或扩展轴状态。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||
|
||||
/// <summary>
|
||||
/// 获取前 6 个机器人关节角度，单位为度。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧尾部状态槽位。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> TailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取原始尾部状态字。当前抓包中恒为 [2,0,0,1]，语义暂不强行推断。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> RawTailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 0 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord0 => _tailWords[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 1 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord1 => _tailWords[1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 2 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord2 => _tailWords[2];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 3 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord3 => _tailWords[3];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
|
||||
/// </summary>
|
||||
public static class FanucStateProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 状态通道抓包确认的完整帧长度。
|
||||
/// </summary>
|
||||
public const int StateFrameLength = 90;
|
||||
|
||||
/// <summary>
|
||||
/// 解析 TCP 10010 状态通道中的单个完整状态帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
/// <returns>状态帧解析结果。</returns>
|
||||
public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
ValidateFrame(frame);
|
||||
|
||||
var pose = new double[6];
|
||||
var jointOrExtensionValues = new double[9];
|
||||
var tailWords = new uint[4];
|
||||
|
||||
// 状态帧采用固定布局，偏移来自抓包与 StateServer 逆向结论。
|
||||
for (var index = 0; index < pose.Length; index++)
|
||||
{
|
||||
pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < jointOrExtensionValues.Length; index++)
|
||||
{
|
||||
jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < tailWords.Length; index++)
|
||||
{
|
||||
tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
|
||||
}
|
||||
|
||||
return new FanucStateFrame(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
|
||||
pose,
|
||||
jointOrExtensionValues,
|
||||
tailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态帧的长度、magic 和长度字段。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
private static void ValidateFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length != StateFrameLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度字段与实际长度不一致。");
|
||||
}
|
||||
}
|
||||
}
|
||||
1207
src/Flyshot.Server.Host/Controllers/DebugConsoleController.cs
Normal file
1207
src/Flyshot.Server.Host/Controllers/DebugConsoleController.cs
Normal file
File diff suppressed because it is too large
Load Diff
25
src/Flyshot.Server.Host/Controllers/HealthController.cs
Normal file
25
src/Flyshot.Server.Host/Controllers/HealthController.cs
Normal file
@@ -0,0 +1,25 @@
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供宿主基础探活与诊断接口。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class HealthController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// 返回宿主健康状态。
|
||||
/// </summary>
|
||||
/// <returns>固定的健康检查 JSON。</returns>
|
||||
[HttpGet("/healthz")]
|
||||
public IActionResult GetHealth()
|
||||
{
|
||||
return Ok(new
|
||||
{
|
||||
status = "ok",
|
||||
service = "flyshot-server-host"
|
||||
});
|
||||
}
|
||||
}
|
||||
951
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
951
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
@@ -0,0 +1,951 @@
|
||||
using System.Text.Json;
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供对 `flyshot-uaes-interface` 既有 FastAPI HTTP 路由层的一比一 MVC 兼容控制器。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("ControllerClient 兼容")]
|
||||
public sealed class LegacyHttpApiController : ControllerBase
|
||||
{
|
||||
private readonly IControllerClientCompatService _compatService;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化旧 HTTP 兼容控制器。
|
||||
/// </summary>
|
||||
/// <param name="compatService">ControllerClient 兼容服务。</param>
|
||||
public LegacyHttpApiController(IControllerClientCompatService compatService)
|
||||
{
|
||||
_compatService = compatService ?? throw new ArgumentNullException(nameof(compatService));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧根路径探活接口。
|
||||
/// </summary>
|
||||
/// <returns>旧 HTTP 服务约定的 Hello World 响应。</returns>
|
||||
[HttpGet("/")]
|
||||
public IActionResult Root()
|
||||
{
|
||||
return Ok(new { message = "Hello World" });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/connect_server/` 路由；在 replacement 宿主中仅记录调用方期望连接的地址。
|
||||
/// </summary>
|
||||
/// <param name="server_ip">旧客户端传入的服务端 IP。</param>
|
||||
/// <param name="port">旧客户端传入的服务端端口。</param>
|
||||
/// <returns>与旧 FastAPI 层一致的状态响应。</returns>
|
||||
[HttpPost("/connect_server/")]
|
||||
public IActionResult ConnectServer([FromQuery] string server_ip, [FromQuery] int port)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.ConnectServer(server_ip, port);
|
||||
return Ok(new { status = "connected" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("Connect Server failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `GetServerVersion` 版本查询语义。
|
||||
/// </summary>
|
||||
/// <returns>服务端版本号。</returns>
|
||||
[HttpGet("/get_server_version/")]
|
||||
public IActionResult GetServerVersion()
|
||||
{
|
||||
return Ok(new { server_version = _compatService.GetServerVersion() });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `GetClientVersion` 版本查询语义。
|
||||
/// </summary>
|
||||
/// <returns>客户端版本号。</returns>
|
||||
[HttpGet("/get_client_version/")]
|
||||
public IActionResult GetClientVersion()
|
||||
{
|
||||
return Ok(new { client_version = _compatService.GetClientVersion() });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/setup_robot/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="robot_name">旧 HTTP 层使用的机器人名称。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/setup_robot/")]
|
||||
public IActionResult SetupRobot([FromQuery] string robot_name)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetUpRobot(robot_name);
|
||||
return Ok(new { status = "robot setup" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetUpRobot failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `SetUpRobotFromEnv(env_file)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="env_file">环境文件路径。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/setup_robot_from_env/")]
|
||||
public IActionResult SetupRobotFromEnv([FromQuery] string env_file)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetUpRobotFromEnv(env_file);
|
||||
return Ok(new { status = "robot setup" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetUpRobotFromEnv failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `SetShowTCP(is_show, axis_length, axis_size)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="is_show">是否显示 TCP。</param>
|
||||
/// <param name="axis_length">坐标轴长度。</param>
|
||||
/// <param name="axis_size">坐标轴线宽。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/set_show_tcp/")]
|
||||
public IActionResult SetShowTcp(
|
||||
[FromQuery] bool is_show = true,
|
||||
[FromQuery] double axis_length = 0.1,
|
||||
[FromQuery] int axis_size = 2)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetShowTcp(is_show, axis_length, axis_size);
|
||||
return Ok(new { status = "show TCP set" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetShowTCP failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/is_setup/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>当前机器人是否完成初始化。</returns>
|
||||
[HttpGet("/is_setup/")]
|
||||
public IActionResult IsSetup()
|
||||
{
|
||||
return Ok(new { is_setup = _compatService.IsSetUp });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="buffer_size">控制器执行缓冲区大小。</param>
|
||||
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
||||
[HttpGet("/enable_robot/")]
|
||||
public IActionResult EnableRobot([FromQuery] int buffer_size = 2)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.EnableRobot(buffer_size);
|
||||
return Ok(new { enable_robot = true });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("EnableRobot failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/disable_robot/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
||||
[HttpGet("/disable_robot/")]
|
||||
public IActionResult DisableRobot()
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.DisableRobot();
|
||||
return Ok(new { disable_robot = true });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("DisableRobot failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供与旧客户端 <c>StopMove</c> 语义对应的 HTTP 端点。
|
||||
/// </summary>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpGet("/stop_move/")]
|
||||
public IActionResult StopMove()
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.StopMove();
|
||||
return Ok(new { status = "move stopped" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("StopMove failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/set_active_controller/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否切到仿真控制器。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/set_active_controller/")]
|
||||
public IActionResult SetActiveController([FromQuery] bool sim)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetActiveController(sim);
|
||||
return Ok(new { status = "active controller set" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetActiveController failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/connect_robot/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制器 IP。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/connect_robot/")]
|
||||
public IActionResult ConnectRobot([FromQuery] string ip)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.Connect(ip);
|
||||
return Ok(new { status = "robot connected" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("Connect failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供与旧客户端 <c>Disconnect</c> 语义对应的 HTTP 端点。
|
||||
/// </summary>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/disconnect_robot/")]
|
||||
public IActionResult DisconnectRobot()
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.Disconnect();
|
||||
return Ok(new { status = "robot disconnected" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("Disconnect failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/robot_info/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>旧 HTTP 层聚合的机器人元信息。</returns>
|
||||
[HttpGet("/robot_info/")]
|
||||
public IActionResult GetRobotInfo()
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new
|
||||
{
|
||||
name = _compatService.GetRobotName(),
|
||||
server_version = _compatService.ServerVersion,
|
||||
dof = _compatService.GetDegreesOfFreedom(),
|
||||
speed_ratio = _compatService.GetSpeedRatio()
|
||||
});
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetRobotInfo failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/set_tcp/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="tcp_data">三维 TCP 请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/set_tcp/")]
|
||||
public IActionResult SetTcp([FromBody] LegacyTcpRequest tcp_data)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetTcp(tcp_data.x, tcp_data.y, tcp_data.z);
|
||||
return Ok(new { status = "TCP set" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetTCP failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/get_tcp/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>当前 TCP 三维坐标。</returns>
|
||||
[HttpGet("/get_tcp/")]
|
||||
public IActionResult GetTcp()
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new { tcp = _compatService.GetTcp() });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetTCP failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/set_io/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="value">IO 值。</param>
|
||||
/// <param name="io_type">IO 类型字符串。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/set_io/")]
|
||||
public IActionResult SetIo([FromQuery] int port, [FromQuery] bool value, [FromQuery] string io_type)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetIo(port, value, io_type);
|
||||
return Ok(new { status = "IO set" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SetDigitalOutput failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/get_io/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="io_type">IO 类型字符串。</param>
|
||||
/// <returns>当前 IO 值。</returns>
|
||||
[HttpGet("/get_io/")]
|
||||
public IActionResult GetIo([FromQuery] int port, [FromQuery] string io_type)
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new { value = _compatService.GetIo(port, io_type) });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetDigitalOutput failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/get_joint_position/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>旧 HTTP 层定义的关节位置 JSON 外形。</returns>
|
||||
[HttpGet("/get_joint_position/")]
|
||||
public IActionResult GetJointPosition()
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new { success = true, points = _compatService.GetJointPositions() });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetJointPosition failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/move_joint/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="joint_data">关节位置请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/move_joint/")]
|
||||
public IActionResult MoveJoint([FromBody] LegacyJointPositionRequest joint_data)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.MoveJoint(joint_data.joints);
|
||||
return Ok(new { status = "robot moved" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("MoveJoint failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="request">IK 请求体。</param>
|
||||
/// <returns>IK 结果。</returns>
|
||||
[HttpPost("/get_nearest_ik/")]
|
||||
public IActionResult GetNearestIk([FromBody] LegacyNearestIkRequest request)
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new { success = true, ik = _compatService.GetNearestIk(request.pose, request.seed) });
|
||||
}
|
||||
catch (NotSupportedException exception)
|
||||
{
|
||||
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetNearestIK failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/list_flyShotTraj/` 路由。
|
||||
/// </summary>
|
||||
/// <returns>已上传飞拍轨迹名称列表。</returns>
|
||||
[HttpGet("/list_flyShotTraj/")]
|
||||
public IActionResult ListFlyshotTrajectories()
|
||||
{
|
||||
var names = _compatService.ListTrajectoryNames();
|
||||
if (names.Count == 0)
|
||||
{
|
||||
return LegacyBadRequest("ListFlyShotTraj failed");
|
||||
}
|
||||
|
||||
return Ok(new { flyshot_trajs = names });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/execute_trajectory/` 路由，并接受两种历史请求体形状。
|
||||
/// </summary>
|
||||
/// <param name="waypoints">轨迹请求体。</param>
|
||||
/// <param name="method">查询字符串中的 method 覆盖值（兼容历史调用方式）。</param>
|
||||
/// <param name="save_traj">查询字符串中的 save_traj 覆盖值（兼容历史调用方式）。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/execute_trajectory/")]
|
||||
public IActionResult ExecuteTrajectory(
|
||||
[FromBody] JsonElement waypoints,
|
||||
[FromQuery] string? method = null,
|
||||
[FromQuery] bool? save_traj = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var request = ParseExecuteTrajectoryRequest(waypoints, method, save_traj);
|
||||
_compatService.ExecuteTrajectory(
|
||||
request.Waypoints,
|
||||
new TrajectoryExecutionOptions(request.Method, request.SaveTrajectory));
|
||||
return Ok(new { status = "trajectory executed" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("ExecuteTrajectory failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/upload_flyshot/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="trajectory_data">飞拍上传请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/upload_flyshot/")]
|
||||
public IActionResult UploadFlyshot([FromBody] LegacyFlightTrajectoryRequest trajectory_data)
|
||||
{
|
||||
if (trajectory_data.shot_flags.Count != trajectory_data.waypoints.Count)
|
||||
{
|
||||
return LegacyValidationError("shot_flags长度必须与路点数量相同");
|
||||
}
|
||||
|
||||
if (trajectory_data.offset_values.Count != trajectory_data.waypoints.Count)
|
||||
{
|
||||
return LegacyValidationError("offset_values长度必须与路点数量相同");
|
||||
}
|
||||
|
||||
if (trajectory_data.addrs.Count != trajectory_data.waypoints.Count)
|
||||
{
|
||||
return LegacyValidationError("addrs长度必须与路点数量相同");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var trajectory = new ControllerClientCompatUploadedTrajectory(
|
||||
name: trajectory_data.name,
|
||||
waypoints: trajectory_data.waypoints,
|
||||
shotFlags: trajectory_data.shot_flags,
|
||||
offsetValues: trajectory_data.offset_values.Select(static value => (int)value),
|
||||
addressGroups: trajectory_data.addrs);
|
||||
|
||||
_compatService.UploadTrajectory(trajectory);
|
||||
return Ok(new { status = "FlyShot uploaded" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("UploadFlyShotTraj failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/execute_flyshot/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="data">包含轨迹名称和执行参数的请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/execute_flyshot/")]
|
||||
public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.ExecuteTrajectoryByName(
|
||||
data.name,
|
||||
new FlyshotExecutionOptions(
|
||||
moveToStart: data.move_to_start,
|
||||
method: data.method,
|
||||
saveTrajectory: data.save_traj,
|
||||
useCache: data.use_cache));
|
||||
return Ok(new { status = "FlyShot executed", success = true });
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return StatusCode(StatusCodes.Status500InternalServerError, new { detail = exception.Message });
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `SaveTrajInfo(name, method)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="request">轨迹保存请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/save_traj_info/")]
|
||||
public IActionResult SaveTrajectoryInfo([FromBody] LegacyTrajectoryInfoRequest request)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SaveTrajectoryInfo(request.name, request.method);
|
||||
return Ok(new { status = "trajectory info saved", success = true });
|
||||
}
|
||||
catch (NotSupportedException exception)
|
||||
{
|
||||
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("SaveTrajInfo failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `IsFlyShotTrajValid(time, name, method, save_traj)` 参数形状。
|
||||
/// </summary>
|
||||
/// <param name="request">轨迹有效性检查请求体。</param>
|
||||
/// <returns>有效性和轨迹时长。</returns>
|
||||
[HttpPost("/is_flyShotTrajValid/")]
|
||||
public IActionResult IsFlyshotTrajectoryValid([FromBody] LegacyFlyshotValidationRequest request)
|
||||
{
|
||||
try
|
||||
{
|
||||
var isValid = _compatService.IsFlyshotTrajectoryValid(
|
||||
out var duration,
|
||||
request.name,
|
||||
request.method,
|
||||
request.save_traj);
|
||||
|
||||
return Ok(new { success = isValid, valid = isValid, time = duration.TotalSeconds });
|
||||
}
|
||||
catch (NotSupportedException exception)
|
||||
{
|
||||
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("IsFlyShotTrajValid failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/set_speedRatio/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="data">速度倍率请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/set_speedRatio/")]
|
||||
public IActionResult SetSpeedRatio([FromBody] LegacySpeedRatioRequest data)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.SetSpeedRatio(data.speed);
|
||||
return Ok(new { message = "set_speedRatio executed", returnCode = 0 });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("set_speedRatio failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/delete_flyshot/` 路由。
|
||||
/// </summary>
|
||||
/// <param name="request">包含轨迹名称的请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||
[HttpPost("/delete_flyshot/")]
|
||||
public IActionResult DeleteFlyshot([FromBody] LegacyNameRequest request)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.DeleteTrajectory(request.name);
|
||||
return Ok(new { status = "FlyShot deleted" });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("DeleteFlyShotTraj failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/init_mpc_robt` 路由，保留历史拼写。
|
||||
/// </summary>
|
||||
/// <param name="data">初始化请求体。</param>
|
||||
/// <returns>旧 FastAPI 层风格的初始化结果。</returns>
|
||||
[HttpPost("/init_mpc_robt")]
|
||||
public IActionResult InitMpcRobot([FromBody] LegacyInitMpcRobotRequest data)
|
||||
{
|
||||
try
|
||||
{
|
||||
_compatService.ConnectServer(data.server_ip, data.port);
|
||||
_compatService.SetUpRobot(data.robot_name);
|
||||
if (!_compatService.IsSetUp)
|
||||
{
|
||||
return LegacyBadRequest("Robot not setup");
|
||||
}
|
||||
|
||||
_compatService.SetActiveController(data.sim);
|
||||
_compatService.Connect(data.robot_ip);
|
||||
_compatService.EnableRobot(2);
|
||||
return Ok(new { message = "init_Success", returnCode = 0 });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("Connect Server failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容旧 `/get_pose` 路由。
|
||||
/// </summary>
|
||||
/// <returns>当前末端位姿数组。</returns>
|
||||
[HttpGet("/get_pose")]
|
||||
public IActionResult GetPose()
|
||||
{
|
||||
try
|
||||
{
|
||||
return Ok(new { pose = _compatService.GetPose() });
|
||||
}
|
||||
catch
|
||||
{
|
||||
return LegacyBadRequest("GetPose failed");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析旧 `/execute_trajectory/` 的完整参数形状。
|
||||
/// </summary>
|
||||
/// <param name="payload">原始 JSON 请求体。</param>
|
||||
/// <param name="queryMethod">查询字符串中的 method 覆盖值。</param>
|
||||
/// <param name="querySaveTrajectory">查询字符串中的 save_traj 覆盖值。</param>
|
||||
/// <returns>统一后的路点和执行参数。</returns>
|
||||
private static (
|
||||
IReadOnlyList<IReadOnlyList<double>> Waypoints,
|
||||
string Method,
|
||||
bool SaveTrajectory) ParseExecuteTrajectoryRequest(
|
||||
JsonElement payload,
|
||||
string? queryMethod,
|
||||
bool? querySaveTrajectory)
|
||||
{
|
||||
string method = queryMethod ?? "icsp";
|
||||
bool saveTrajectory = querySaveTrajectory ?? false;
|
||||
|
||||
if (payload.ValueKind == JsonValueKind.Object)
|
||||
{
|
||||
if (payload.TryGetProperty("method", out var methodElement) && methodElement.ValueKind == JsonValueKind.String)
|
||||
{
|
||||
method = methodElement.GetString() ?? method;
|
||||
}
|
||||
|
||||
if (payload.TryGetProperty("save_traj", out var saveTrajectoryElement))
|
||||
{
|
||||
saveTrajectory = saveTrajectoryElement.GetBoolean();
|
||||
}
|
||||
|
||||
if (!payload.TryGetProperty("waypoints", out var waypointElement))
|
||||
{
|
||||
throw new InvalidOperationException("ExecuteTrajectory request body must include waypoints.");
|
||||
}
|
||||
|
||||
return (ParseLegacyTrajectoryWaypoints(waypointElement), method, saveTrajectory);
|
||||
}
|
||||
|
||||
return (ParseLegacyTrajectoryWaypoints(payload), method, saveTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析旧 `/execute_trajectory/` 可能出现的两种历史请求体形状。
|
||||
/// </summary>
|
||||
/// <param name="waypoints">原始 JSON 请求体。</param>
|
||||
/// <returns>统一后的关节路点集合。</returns>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> ParseLegacyTrajectoryWaypoints(JsonElement waypoints)
|
||||
{
|
||||
if (waypoints.ValueKind != JsonValueKind.Array)
|
||||
{
|
||||
throw new InvalidOperationException("ExecuteTrajectory request body must be an array.");
|
||||
}
|
||||
|
||||
var parsedWaypoints = new List<IReadOnlyList<double>>();
|
||||
foreach (var waypointElement in waypoints.EnumerateArray())
|
||||
{
|
||||
if (waypointElement.ValueKind == JsonValueKind.Array)
|
||||
{
|
||||
parsedWaypoints.Add(waypointElement.EnumerateArray().Select(static value => value.GetDouble()).ToArray());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (waypointElement.ValueKind == JsonValueKind.Object && waypointElement.TryGetProperty("joints", out var jointElement))
|
||||
{
|
||||
parsedWaypoints.Add(jointElement.EnumerateArray().Select(static value => value.GetDouble()).ToArray());
|
||||
continue;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("Unsupported waypoint payload shape.");
|
||||
}
|
||||
|
||||
return parsedWaypoints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造与旧 FastAPI `HTTPException(status_code=400, detail=...)` 等价的响应。
|
||||
/// </summary>
|
||||
/// <param name="detail">错误详情文本。</param>
|
||||
/// <returns>400 JSON 响应。</returns>
|
||||
private BadRequestObjectResult LegacyBadRequest(string detail)
|
||||
{
|
||||
return BadRequest(new { detail });
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造与旧 FastAPI `422` 输入校验失败等价的响应。
|
||||
/// </summary>
|
||||
/// <param name="detail">错误详情文本。</param>
|
||||
/// <returns>422 JSON 响应。</returns>
|
||||
private ObjectResult LegacyValidationError(string detail)
|
||||
{
|
||||
return StatusCode(StatusCodes.Status422UnprocessableEntity, new { detail });
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/set_tcp/` 路由使用的三维 TCP 请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyTcpRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置 TCP X。
|
||||
/// </summary>
|
||||
public double x { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 TCP Y。
|
||||
/// </summary>
|
||||
public double y { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 TCP Z。
|
||||
/// </summary>
|
||||
public double z { get; init; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/move_joint/` 路由使用的关节请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyJointPositionRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置目标关节数组。
|
||||
/// </summary>
|
||||
public List<double> joints { get; init; } = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyFlightTrajectoryRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置地址组集合。
|
||||
/// </summary>
|
||||
public List<List<int>> addrs { get; init; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置飞拍轨迹名称。
|
||||
/// </summary>
|
||||
public string name { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置偏移周期集合。
|
||||
/// </summary>
|
||||
public List<double> offset_values { get; init; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置拍照标志集合。
|
||||
/// </summary>
|
||||
public List<bool> shot_flags { get; init; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置关节路点集合。
|
||||
/// </summary>
|
||||
public List<List<double>> waypoints { get; init; } = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/execute_flyshot/` 与 `/delete_flyshot/` 路由使用的名称请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyNameRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹名称。
|
||||
/// </summary>
|
||||
public string name { get; init; } = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/execute_flyshot/` 路由使用的完整执行请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyExecuteFlyshotRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹名称。
|
||||
/// </summary>
|
||||
public string name { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置是否先移动到轨迹起点。
|
||||
/// </summary>
|
||||
public bool move_to_start { get; init; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹生成方法。
|
||||
/// </summary>
|
||||
public string method { get; init; } = "icsp";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool save_traj { get; init; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置是否复用轨迹缓存。
|
||||
/// </summary>
|
||||
public bool use_cache { get; init; } = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `SaveTrajInfo` 参数形状。
|
||||
/// </summary>
|
||||
public sealed class LegacyTrajectoryInfoRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹名称。
|
||||
/// </summary>
|
||||
public string name { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹生成方法。
|
||||
/// </summary>
|
||||
public string method { get; init; } = "icsp";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `IsFlyShotTrajValid` 参数形状。
|
||||
/// </summary>
|
||||
public sealed class LegacyFlyshotValidationRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹名称。
|
||||
/// </summary>
|
||||
public string name { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置轨迹生成方法。
|
||||
/// </summary>
|
||||
public string method { get; init; } = "icsp";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool save_traj { get; init; } = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `GetNearestIK` 参数形状。
|
||||
/// </summary>
|
||||
public sealed class LegacyNearestIkRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置目标位姿 `[x,y,z,qx,qy,qz,qw]`。
|
||||
/// </summary>
|
||||
public List<double> pose { get; init; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 IK seed 关节数组。
|
||||
/// </summary>
|
||||
public List<double> seed { get; init; } = [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/set_speedRatio/` 路由使用的速度倍率请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacySpeedRatioRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置目标速度倍率。
|
||||
/// </summary>
|
||||
public double speed { get; init; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示旧 `/init_mpc_robt` 路由使用的初始化请求体。
|
||||
/// </summary>
|
||||
public sealed class LegacyInitMpcRobotRequest
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置目标服务端 IP。
|
||||
/// </summary>
|
||||
public string server_ip { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置目标服务端端口。
|
||||
/// </summary>
|
||||
public int port { get; init; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置机器人名称。
|
||||
/// </summary>
|
||||
public string robot_name { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置机器人控制器 IP。
|
||||
/// </summary>
|
||||
public string robot_ip { get; init; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置是否使用仿真控制器；默认 false 连接真机。
|
||||
/// </summary>
|
||||
public bool sim { get; init; }
|
||||
}
|
||||
413
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
413
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
@@ -0,0 +1,413 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供只读状态监控页面和控制器状态快照 API。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class StatusController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// 浏览器端状态监控页面，页面逻辑只读取状态快照，不触发控制动作。
|
||||
/// </summary>
|
||||
private const string StatusPageHtml = """
|
||||
<!doctype html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>Flyshot Replacement 状态监控</title>
|
||||
<style>
|
||||
:root {
|
||||
color-scheme: light;
|
||||
--bg: #f5f7fb;
|
||||
--surface: #ffffff;
|
||||
--line: #d8dee9;
|
||||
--text: #172033;
|
||||
--muted: #5b667a;
|
||||
--accent: #007c89;
|
||||
--good: #12805c;
|
||||
--warn: #b7791f;
|
||||
--bad: #b42318;
|
||||
}
|
||||
|
||||
* {
|
||||
box-sizing: border-box;
|
||||
}
|
||||
|
||||
body {
|
||||
margin: 0;
|
||||
min-height: 100vh;
|
||||
background: var(--bg);
|
||||
color: var(--text);
|
||||
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
|
||||
font-size: 15px;
|
||||
letter-spacing: 0;
|
||||
}
|
||||
|
||||
header {
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.topbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 16px;
|
||||
width: min(1180px, calc(100% - 32px));
|
||||
margin: 0 auto;
|
||||
padding: 18px 0;
|
||||
}
|
||||
|
||||
h1 {
|
||||
margin: 0;
|
||||
font-size: 22px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
button {
|
||||
min-height: 36px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: var(--accent);
|
||||
color: #ffffff;
|
||||
font: inherit;
|
||||
cursor: pointer;
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
opacity: 0.6;
|
||||
cursor: default;
|
||||
}
|
||||
|
||||
/* 顶部操作区按钮和外链按钮共用同一组视觉样式，便于现场顺手跳转。 */
|
||||
.actions {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.link-button {
|
||||
display: inline-flex;
|
||||
align-items: center;
|
||||
min-height: 36px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: transparent;
|
||||
color: var(--accent);
|
||||
font: inherit;
|
||||
text-decoration: none;
|
||||
}
|
||||
|
||||
.link-button:hover {
|
||||
background: rgba(0, 124, 137, 0.08);
|
||||
}
|
||||
|
||||
main {
|
||||
width: min(1180px, calc(100% - 32px));
|
||||
margin: 22px auto;
|
||||
}
|
||||
|
||||
.summary {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(4, minmax(0, 1fr));
|
||||
gap: 12px;
|
||||
margin-bottom: 16px;
|
||||
}
|
||||
|
||||
.metric,
|
||||
section {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.metric {
|
||||
min-height: 86px;
|
||||
padding: 14px;
|
||||
}
|
||||
|
||||
.label {
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.value {
|
||||
margin-top: 8px;
|
||||
overflow-wrap: anywhere;
|
||||
font-size: 24px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.status-row {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
}
|
||||
|
||||
.dot {
|
||||
width: 12px;
|
||||
height: 12px;
|
||||
flex: 0 0 12px;
|
||||
border-radius: 999px;
|
||||
background: var(--warn);
|
||||
}
|
||||
|
||||
.dot.good {
|
||||
background: var(--good);
|
||||
}
|
||||
|
||||
.dot.bad {
|
||||
background: var(--bad);
|
||||
}
|
||||
|
||||
.grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||
gap: 16px;
|
||||
}
|
||||
|
||||
section h2 {
|
||||
margin: 0;
|
||||
padding: 14px 16px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
font-size: 16px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
dl {
|
||||
display: grid;
|
||||
grid-template-columns: 160px minmax(0, 1fr);
|
||||
gap: 0;
|
||||
margin: 0;
|
||||
padding: 4px 16px 12px;
|
||||
}
|
||||
|
||||
dt,
|
||||
dd {
|
||||
min-height: 36px;
|
||||
margin: 0;
|
||||
padding: 9px 0;
|
||||
border-bottom: 1px solid #edf1f7;
|
||||
}
|
||||
|
||||
dt {
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
dd {
|
||||
overflow-wrap: anywhere;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.empty {
|
||||
color: var(--muted);
|
||||
font-family: inherit;
|
||||
}
|
||||
|
||||
@media (max-width: 820px) {
|
||||
.topbar {
|
||||
align-items: flex-start;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.summary,
|
||||
.grid {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
dl {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
dt {
|
||||
border-bottom: 0;
|
||||
padding-bottom: 2px;
|
||||
}
|
||||
|
||||
dd {
|
||||
padding-top: 2px;
|
||||
}
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<div class="topbar">
|
||||
<h1>Flyshot Replacement 状态监控</h1>
|
||||
<div class="actions">
|
||||
<a class="link-button" href="/debug" target="_blank" rel="noopener">调试接口</a>
|
||||
<button id="refresh" type="button">刷新</button>
|
||||
</div>
|
||||
</div>
|
||||
</header>
|
||||
<main>
|
||||
<div class="summary">
|
||||
<div class="metric">
|
||||
<div class="label">连接状态</div>
|
||||
<div class="value status-row"><span id="state-dot" class="dot"></span><span id="connection-state">--</span></div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">机器人</div>
|
||||
<div id="robot-name" class="value">--</div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">速度倍率</div>
|
||||
<div id="speed-ratio" class="value">--</div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">运动中</div>
|
||||
<div id="motion-state" class="value">--</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="grid">
|
||||
<section>
|
||||
<h2>控制器</h2>
|
||||
<dl>
|
||||
<dt>服务端版本</dt><dd id="server-version">--</dd>
|
||||
<dt>客户端版本</dt><dd id="client-version">--</dd>
|
||||
<dt>已初始化</dt><dd id="setup-state">--</dd>
|
||||
<dt>已使能</dt><dd id="enabled-state">--</dd>
|
||||
<dt>采样时间</dt><dd id="captured-at">--</dd>
|
||||
</dl>
|
||||
</section>
|
||||
<section>
|
||||
<h2>机器人</h2>
|
||||
<dl>
|
||||
<dt>自由度</dt><dd id="dof">--</dd>
|
||||
<dt>关节位置</dt><dd id="joints">--</dd>
|
||||
<dt>TCP 位姿</dt><dd id="pose">--</dd>
|
||||
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
|
||||
</dl>
|
||||
</section>
|
||||
</div>
|
||||
</main>
|
||||
<script>
|
||||
const fields = {
|
||||
connectionState: document.getElementById("connection-state"),
|
||||
stateDot: document.getElementById("state-dot"),
|
||||
robotName: document.getElementById("robot-name"),
|
||||
speedRatio: document.getElementById("speed-ratio"),
|
||||
motionState: document.getElementById("motion-state"),
|
||||
serverVersion: document.getElementById("server-version"),
|
||||
clientVersion: document.getElementById("client-version"),
|
||||
setupState: document.getElementById("setup-state"),
|
||||
enabledState: document.getElementById("enabled-state"),
|
||||
capturedAt: document.getElementById("captured-at"),
|
||||
dof: document.getElementById("dof"),
|
||||
joints: document.getElementById("joints"),
|
||||
pose: document.getElementById("pose"),
|
||||
trajectories: document.getElementById("trajectories"),
|
||||
refresh: document.getElementById("refresh")
|
||||
};
|
||||
|
||||
function formatArray(values) {
|
||||
if (!Array.isArray(values) || values.length === 0) {
|
||||
return "--";
|
||||
}
|
||||
|
||||
return values.map(value => Number(value).toFixed(4)).join(", ");
|
||||
}
|
||||
|
||||
function setDot(connectionState) {
|
||||
fields.stateDot.className = "dot";
|
||||
if (connectionState === "Connected") {
|
||||
fields.stateDot.classList.add("good");
|
||||
} else if (connectionState === "NotConfigured") {
|
||||
fields.stateDot.classList.add("bad");
|
||||
}
|
||||
}
|
||||
|
||||
async function refreshStatus() {
|
||||
fields.refresh.disabled = true;
|
||||
try {
|
||||
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
|
||||
const payload = await response.json();
|
||||
const snapshot = payload.snapshot;
|
||||
|
||||
fields.connectionState.textContent = snapshot.connectionState;
|
||||
fields.robotName.textContent = payload.robotName || "--";
|
||||
fields.speedRatio.textContent = Number(snapshot.speedRatio).toFixed(2);
|
||||
fields.motionState.textContent = snapshot.isInMotion ? "是" : "否";
|
||||
fields.serverVersion.textContent = payload.serverVersion;
|
||||
fields.clientVersion.textContent = payload.clientVersion;
|
||||
fields.setupState.textContent = payload.isSetup ? "是" : "否";
|
||||
fields.enabledState.textContent = snapshot.isEnabled ? "是" : "否";
|
||||
fields.capturedAt.textContent = new Date(snapshot.capturedAt).toLocaleString();
|
||||
fields.dof.textContent = payload.degreesOfFreedom;
|
||||
fields.joints.textContent = formatArray(snapshot.jointPositions);
|
||||
fields.pose.textContent = formatArray(snapshot.cartesianPose);
|
||||
fields.trajectories.textContent = payload.uploadedTrajectories.length > 0
|
||||
? payload.uploadedTrajectories.join(", ")
|
||||
: "--";
|
||||
fields.trajectories.classList.toggle("empty", payload.uploadedTrajectories.length === 0);
|
||||
setDot(snapshot.connectionState);
|
||||
} finally {
|
||||
fields.refresh.disabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
fields.refresh.addEventListener("click", refreshStatus);
|
||||
refreshStatus();
|
||||
window.setInterval(refreshStatus, 2000);
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
""";
|
||||
|
||||
private readonly IControllerClientCompatService _compatService;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态监控控制器。
|
||||
/// </summary>
|
||||
/// <param name="compatService">ControllerClient 兼容层服务。</param>
|
||||
public StatusController(IControllerClientCompatService compatService)
|
||||
{
|
||||
_compatService = compatService ?? throw new ArgumentNullException(nameof(compatService));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回浏览器可直接打开的状态监控页面。
|
||||
/// </summary>
|
||||
/// <returns>HTML 状态页面。</returns>
|
||||
[HttpGet("/status")]
|
||||
public ContentResult GetStatusPage()
|
||||
{
|
||||
return Content(StatusPageHtml, "text/html; charset=utf-8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回当前 ControllerClient 兼容层与控制器运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>面向状态页和外部诊断的 JSON 快照。</returns>
|
||||
[HttpGet("/api/status/snapshot")]
|
||||
public IActionResult GetSnapshot()
|
||||
{
|
||||
var snapshot = _compatService.GetControllerSnapshot();
|
||||
var isSetup = _compatService.IsSetUp;
|
||||
|
||||
// 状态页需要在机器人未初始化时仍能打开，因此只有初始化后才读取机器人元数据。
|
||||
var robotName = isSetup ? _compatService.GetRobotName() : null;
|
||||
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
|
||||
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
Status = "ok",
|
||||
Service = "flyshot-server-host",
|
||||
ServerVersion = _compatService.GetServerVersion(),
|
||||
ClientVersion = _compatService.GetClientVersion(),
|
||||
IsSetup = isSetup,
|
||||
RobotName = robotName,
|
||||
DegreesOfFreedom = degreesOfFreedom,
|
||||
UploadedTrajectories = uploadedTrajectories,
|
||||
Snapshot = snapshot
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,18 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk.Web">
|
||||
<PropertyGroup>
|
||||
<!-- 生成 XML 文档以便 Swashbuckle 把控制器/DTO 上的 /// summary 注释注入 OpenAPI 文档，
|
||||
调试页和 Swagger UI 的端点标题都依赖这一份文档。1591 抑制掉 “缺失 XML 注释” 的噪音。 -->
|
||||
<GenerateDocumentationFile>true</GenerateDocumentationFile>
|
||||
<NoWarn>$(NoWarn);1591</NoWarn>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Swashbuckle.AspNetCore" Version="6.6.2" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
|
||||
27
src/Flyshot.Server.Host/HostCorsOptions.cs
Normal file
27
src/Flyshot.Server.Host/HostCorsOptions.cs
Normal file
@@ -0,0 +1,27 @@
|
||||
namespace Flyshot.Server.Host;
|
||||
|
||||
/// <summary>
|
||||
/// 表示宿主 CORS 策略的标准配置项。
|
||||
/// </summary>
|
||||
public sealed class HostCorsOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置 CORS 策略名称。
|
||||
/// </summary>
|
||||
public string PolicyName { get; set; } = "LegacyHttpApi";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的源列表。
|
||||
/// </summary>
|
||||
public string[] AllowedOrigins { get; set; } = ["*"];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的 HTTP 方法列表。
|
||||
/// </summary>
|
||||
public string[] AllowedMethods { get; set; } = ["GET", "POST", "OPTIONS"];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的请求头列表。
|
||||
/// </summary>
|
||||
public string[] AllowedHeaders { get; set; } = ["*"];
|
||||
}
|
||||
37
src/Flyshot.Server.Host/HostSwaggerOptions.cs
Normal file
37
src/Flyshot.Server.Host/HostSwaggerOptions.cs
Normal file
@@ -0,0 +1,37 @@
|
||||
namespace Flyshot.Server.Host;
|
||||
|
||||
/// <summary>
|
||||
/// 表示宿主 Swagger/OpenAPI 文档的标准配置项。
|
||||
/// </summary>
|
||||
public sealed class HostSwaggerOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置是否启用 Swagger。
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置文档名称。
|
||||
/// </summary>
|
||||
public string DocumentName { get; set; } = "v1";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger 文档标题。
|
||||
/// </summary>
|
||||
public string Title { get; set; } = "Flyshot Replacement HTTP API";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger 文档版本文本。
|
||||
/// </summary>
|
||||
public string Version { get; set; } = "v1";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger JSON 路由模板。
|
||||
/// </summary>
|
||||
public string JsonRouteTemplate { get; set; } = "swagger/{documentName}/swagger.json";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger UI 路由前缀。
|
||||
/// </summary>
|
||||
public string RoutePrefix { get; set; } = "swagger";
|
||||
}
|
||||
@@ -1,12 +1,93 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Server.Host;
|
||||
using Microsoft.Extensions.Options;
|
||||
using Microsoft.OpenApi.Models;
|
||||
using Swashbuckle.AspNetCore.SwaggerGen;
|
||||
|
||||
var builder = WebApplication.CreateBuilder(args);
|
||||
|
||||
builder.Services.Configure<HostSwaggerOptions>(builder.Configuration.GetSection("Swagger"));
|
||||
builder.Services.Configure<HostCorsOptions>(builder.Configuration.GetSection("Cors"));
|
||||
builder.Services.AddControllerClientCompat(builder.Configuration);
|
||||
builder.Services.AddControllers();
|
||||
builder.Services.AddEndpointsApiExplorer();
|
||||
|
||||
var swaggerOptions = builder.Configuration.GetSection("Swagger").Get<HostSwaggerOptions>() ?? new HostSwaggerOptions();
|
||||
builder.Services.AddSwaggerGen(options =>
|
||||
{
|
||||
options.SwaggerDoc(swaggerOptions.DocumentName, new OpenApiInfo
|
||||
{
|
||||
Title = swaggerOptions.Title,
|
||||
Version = swaggerOptions.Version
|
||||
});
|
||||
|
||||
// 把控制器与 DTO 上的 /// summary 注释纳入 OpenAPI 文档；调试页据此渲染端点标题。
|
||||
var xmlDocumentationPath = Path.Combine(AppContext.BaseDirectory, $"{typeof(Program).Assembly.GetName().Name}.xml");
|
||||
if (File.Exists(xmlDocumentationPath))
|
||||
{
|
||||
options.IncludeXmlComments(xmlDocumentationPath, includeControllerXmlComments: true);
|
||||
}
|
||||
});
|
||||
|
||||
var corsOptions = builder.Configuration.GetSection("Cors").Get<HostCorsOptions>() ?? new HostCorsOptions();
|
||||
builder.Services.AddCors(options =>
|
||||
{
|
||||
options.AddPolicy(corsOptions.PolicyName, policyBuilder =>
|
||||
{
|
||||
// 兼容本地调试时最常见的任意源配置，同时保留显式白名单模式。
|
||||
if (corsOptions.AllowedOrigins.Length == 1 && string.Equals(corsOptions.AllowedOrigins[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyOrigin();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithOrigins(corsOptions.AllowedOrigins);
|
||||
}
|
||||
|
||||
if (corsOptions.AllowedMethods.Length == 1 && string.Equals(corsOptions.AllowedMethods[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyMethod();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithMethods(corsOptions.AllowedMethods);
|
||||
}
|
||||
|
||||
if (corsOptions.AllowedHeaders.Length == 1 && string.Equals(corsOptions.AllowedHeaders[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyHeader();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithHeaders(corsOptions.AllowedHeaders);
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
var app = builder.Build();
|
||||
|
||||
app.MapGet("/", () => Results.Redirect("/healthz"));
|
||||
app.MapGet("/healthz", () => Results.Ok(new
|
||||
var resolvedSwaggerOptions = app.Services.GetRequiredService<IOptions<HostSwaggerOptions>>().Value;
|
||||
var resolvedCorsOptions = app.Services.GetRequiredService<IOptions<HostCorsOptions>>().Value;
|
||||
|
||||
if (resolvedSwaggerOptions.Enabled)
|
||||
{
|
||||
status = "ok",
|
||||
service = "flyshot-server-host"
|
||||
}));
|
||||
app.UseSwagger(options =>
|
||||
{
|
||||
options.RouteTemplate = resolvedSwaggerOptions.JsonRouteTemplate;
|
||||
});
|
||||
|
||||
app.UseSwaggerUI(options =>
|
||||
{
|
||||
options.RoutePrefix = resolvedSwaggerOptions.RoutePrefix;
|
||||
options.SwaggerEndpoint(
|
||||
$"/swagger/{resolvedSwaggerOptions.DocumentName}/swagger.json",
|
||||
$"{resolvedSwaggerOptions.Title} {resolvedSwaggerOptions.Version}");
|
||||
options.DocumentTitle = resolvedSwaggerOptions.Title;
|
||||
});
|
||||
}
|
||||
|
||||
app.UseCors(resolvedCorsOptions.PolicyName);
|
||||
app.MapControllers();
|
||||
|
||||
app.Run();
|
||||
|
||||
|
||||
@@ -4,5 +4,26 @@
|
||||
"Default": "Information",
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
},
|
||||
"ControllerClientCompat": {
|
||||
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
|
||||
},
|
||||
"Swagger": {
|
||||
"Enabled": true
|
||||
},
|
||||
"Cors": {
|
||||
"PolicyName": "LegacyHttpApi",
|
||||
"AllowedOrigins": [
|
||||
"http://localhost:3000",
|
||||
"http://127.0.0.1:3000"
|
||||
],
|
||||
"AllowedMethods": [
|
||||
"GET",
|
||||
"POST",
|
||||
"OPTIONS"
|
||||
],
|
||||
"AllowedHeaders": [
|
||||
"*"
|
||||
]
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,5 +5,30 @@
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
},
|
||||
"ControllerClientCompat": {
|
||||
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
|
||||
},
|
||||
"Swagger": {
|
||||
"Enabled": true,
|
||||
"DocumentName": "v1",
|
||||
"Title": "Flyshot Replacement HTTP API",
|
||||
"Version": "v1",
|
||||
"JsonRouteTemplate": "swagger/{documentName}/swagger.json",
|
||||
"RoutePrefix": "swagger"
|
||||
},
|
||||
"Cors": {
|
||||
"PolicyName": "LegacyHttpApi",
|
||||
"AllowedOrigins": [
|
||||
"*"
|
||||
],
|
||||
"AllowedMethods": [
|
||||
"GET",
|
||||
"POST",
|
||||
"OPTIONS"
|
||||
],
|
||||
"AllowedHeaders": [
|
||||
"*"
|
||||
]
|
||||
},
|
||||
"AllowedHosts": "*"
|
||||
}
|
||||
|
||||
@@ -26,12 +26,12 @@ public sealed class ConfigCompatibilityTests
|
||||
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);
|
||||
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs);
|
||||
Assert.Equal("EOL10_EAU_0", program.Name);
|
||||
Assert.Equal(45, program.Waypoints.Count);
|
||||
Assert.Equal(42, program.ShotWaypointCount);
|
||||
Assert.Empty(program.AddressGroups[0].Addresses);
|
||||
Assert.Equal([8, 7], program.AddressGroups[1].Addresses);
|
||||
Assert.Equal([4, 3], program.AddressGroups[1].Addresses);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -114,6 +114,27 @@ public sealed class DomainModelTests
|
||||
Assert.Empty(snapshot.JointPositions);
|
||||
Assert.Empty(snapshot.CartesianPose);
|
||||
Assert.Empty(snapshot.ActiveAlarms);
|
||||
Assert.Empty(snapshot.StateTailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证控制器快照会保留 TCP 10010 尾部状态字作为诊断字段。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerStateSnapshot_CopiesStateTailWordsForDiagnostics()
|
||||
{
|
||||
var snapshot = new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
|
||||
connectionState: "Connected",
|
||||
isEnabled: true,
|
||||
isInMotion: false,
|
||||
speedRatio: 1.0,
|
||||
stateTailWords: [2u, 0u, 0u, 1u]);
|
||||
|
||||
var json = JsonSerializer.Serialize(snapshot);
|
||||
|
||||
Assert.Equal([2u, 0u, 0u, 1u], snapshot.StateTailWords);
|
||||
Assert.Contains("\"stateTailWords\":[2,0,0,1]", json);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
319
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs
Normal file
319
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs
Normal file
@@ -0,0 +1,319 @@
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC TCP 10012 命令客户端的帧收发与响应解析。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandClientTests : IDisposable
|
||||
{
|
||||
private readonly TcpListener _listener;
|
||||
private readonly CancellationTokenSource _cts = new();
|
||||
|
||||
/// <summary>
|
||||
/// 在随机可用端口启动本地模拟控制器。
|
||||
/// </summary>
|
||||
public FanucCommandClientTests()
|
||||
{
|
||||
_listener = new TcpListener(IPAddress.Loopback, 0);
|
||||
_listener.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取分配给本地模拟控制器的端口。
|
||||
/// </summary>
|
||||
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
|
||||
|
||||
/// <summary>
|
||||
/// 清理模拟控制器和取消源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
_cts.Cancel();
|
||||
_listener.Stop();
|
||||
_cts.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证命令客户端可以连接本地模拟控制器。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ConnectAsync_ConnectsToLocalListener()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var acceptTask = _listener.AcceptTcpClientAsync();
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
Assert.True(client.IsConnected);
|
||||
// 确保模拟侧也完成握手
|
||||
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 StopProgram 命令帧与抓包样本一致，并能解析成功响应。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StopProgramAsync_SendsCorrectFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
|
||||
Convert.FromHexString("646f7a0000001200002103000000007a6f64"),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.StopProgramAsync("RVBUSTSM", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(FanucCommandMessageIds.StopProgram, response.MessageId);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 ResetRobot 空命令帧能正确发送并解析结果响应。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ResetRobotAsync_SendsEmptyCommandAndParsesResponse()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var expectedFrame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||
var responseFrame = Convert.FromHexString("646f7a0000001200002100000000007a6f64");
|
||||
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.ResetRobotAsync(_cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(FanucCommandMessageIds.ResetRobot, response.MessageId);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetProgramStatus 命令帧能正确发送并解析程序状态响应。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetProgramStatusAsync_SendsFrameAndParsesStatusResponse()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
|
||||
var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
|
||||
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetProgramStatusAsync("RVBUSTSM", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, response.MessageId);
|
||||
Assert.Equal(1u, response.ProgramStatus);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 StartProgram 命令帧能正确发送并解析成功响应。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartProgramAsync_SendsCorrectFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StartProgram, "RVBUSTSM");
|
||||
var responseFrame = Convert.FromHexString("646f7a0000001200002102000000007a6f64");
|
||||
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.StartProgramAsync("RVBUSTSM", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(FanucCommandMessageIds.StartProgram, response.MessageId);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetSpeedRatio 发送空业务体命令，并按 ratio_int / 100.0 解析倍率。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetSpeedRatioAsync_SendsFrameAndParsesRatio()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetSpeedRatioCommand(),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetSpeedRatio, Convert.FromHexString("0000005a00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetSpeedRatioAsync(_cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(0.9, response.Ratio, precision: 6);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetSpeedRatio 会把 double 倍率夹到 0..100 的整数百分比后下发。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetSpeedRatioAsync_SendsClampedPercentAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetSpeedRatioCommand(2.0),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetSpeedRatio, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetSpeedRatioAsync(2.0, _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetTcp 会发送 tcp_id 请求，并解析 result_code + tcp_id + 7 个 float 位姿。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetTcpAsync_SendsFrameAndParsesPose()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetTcpCommand(1),
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetTcp,
|
||||
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetTcpAsync(1, _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], response.Pose);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetTcp 会按 tcp_id + 7 个 float 位姿下发并解析结果码。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetTcpAsync_SendsFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetTcp, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetTcpAsync(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetIo 会按 io_type、io_index 顺序请求，并解析 float IO 值。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetIoAsync_SendsFrameAndParsesValue()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetIo, Convert.FromHexString("000000003f800000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetIoAsync(7, "DO", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.True(response.Value);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetIo 会按 io_type、io_index、float value 顺序下发并解析结果码。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetIoAsync_SendsFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetIo, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetIoAsync(7, true, "DO", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证命令响应 result_code 非零时，客户端会抛出可诊断异常而不是让上层误判成功。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StopProgramAsync_NonZeroResultCode_ThrowsDiagnosticException()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM"),
|
||||
Convert.FromHexString("646f7a00000012000021030000002a7a6f64"),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var exception = await Assert.ThrowsAsync<InvalidOperationException>(
|
||||
() => client.StopProgramAsync("RVBUSTSM", _cts.Token));
|
||||
|
||||
Assert.Contains("0x2103", exception.Message);
|
||||
Assert.Contains("42", exception.Message);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证在连接前调用命令会抛出 InvalidOperationException。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SendProgramCommandAsync_BeforeConnect_Throws()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
await Assert.ThrowsAsync<InvalidOperationException>(
|
||||
() => client.StopProgramAsync("RVBUSTSM", _cts.Token));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动模拟控制器，接收一条请求帧并比对期望内容，然后返回预设响应。
|
||||
/// </summary>
|
||||
private async Task RunSingleResponseControllerAsync(
|
||||
byte[] expectedFrame,
|
||||
byte[] responseFrame,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||
await using var stream = controller.GetStream();
|
||||
|
||||
var buffer = new byte[expectedFrame.Length];
|
||||
await ReadExactAsync(stream, buffer, cancellationToken);
|
||||
Assert.Equal(expectedFrame, buffer);
|
||||
|
||||
await stream.WriteAsync(responseFrame, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取指定长度的字节。
|
||||
/// </summary>
|
||||
private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
var totalRead = 0;
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken);
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("模拟控制器读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
}
|
||||
96
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
Normal file
96
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
Normal file
@@ -0,0 +1,96 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeDenseTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_SimulationMode_FallsBackToSinglePoint()
|
||||
{
|
||||
var runtime = new FanucControllerRuntime();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
runtime.SetActiveController(sim: true);
|
||||
runtime.Connect("192.168.10.101");
|
||||
runtime.EnableRobot(bufferSize: 2);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
||||
new[] { 0.008, 0.11, 0.21, 0.31, 0.41, 0.51, 0.61 },
|
||||
new[] { 0.016, 0.12, 0.22, 0.32, 0.42, 0.52, 0.62 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [0.12, 0.22, 0.32, 0.42, 0.52, 0.62]);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
Assert.False(snapshot.IsInMotion);
|
||||
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StopMove_DoesNotThrowWhenNoSendTaskRunning()
|
||||
{
|
||||
var runtime = new FanucControllerRuntime();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
runtime.SetActiveController(sim: true);
|
||||
runtime.Connect("192.168.10.101");
|
||||
runtime.EnableRobot(bufferSize: 2);
|
||||
|
||||
var exception = Record.Exception(() => runtime.StopMove());
|
||||
Assert.Null(exception);
|
||||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(new[] { 0 }, (ushort)1)]
|
||||
[InlineData(new[] { 7 }, (ushort)128)]
|
||||
[InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
|
||||
[InlineData(new[] { 15 }, (ushort)32768)]
|
||||
[InlineData(new int[] { }, (ushort)0)]
|
||||
public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
|
||||
{
|
||||
var group = new IoAddressGroup(addresses);
|
||||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||||
Assert.Equal(expected, actual);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ComputeIoValue_IgnoresOutOfRangeAddresses()
|
||||
{
|
||||
var group = new IoAddressGroup([0, 16, 7]);
|
||||
var actual = FanucControllerRuntime.ComputeIoValue(group);
|
||||
Assert.Equal((ushort)(1 | 128), actual);
|
||||
}
|
||||
}
|
||||
219
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
219
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
@@ -0,0 +1,219 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519ClientTests : IDisposable
|
||||
{
|
||||
private readonly UdpClient _server;
|
||||
private readonly CancellationTokenSource _cts = new();
|
||||
|
||||
/// <summary>
|
||||
/// 在随机可用端口启动本地 UDP 模拟控制器。
|
||||
/// </summary>
|
||||
public FanucJ519ClientTests()
|
||||
{
|
||||
_server = new UdpClient(0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取分配给本地模拟控制器的端口。
|
||||
/// </summary>
|
||||
private int Port => ((IPEndPoint)_server.Client.LocalEndPoint!).Port;
|
||||
|
||||
/// <summary>
|
||||
/// 清理模拟控制器和取消源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
_cts.Cancel();
|
||||
_server.Dispose();
|
||||
_cts.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证连接时会发送初始化包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ConnectAsync_SendsInitPacket()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
var receiveTask = _server.ReceiveAsync(_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
Assert.True(client.IsConnected);
|
||||
var result = await receiveTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.ControlPacketLength, result.Buffer.Length);
|
||||
Assert.Equal(Convert.FromHexString("0000000000000001"), result.Buffer);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证启动运动后能按周期发送命令包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_SendsPeriodicCommands()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
// 接收并丢弃初始化包。
|
||||
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||
|
||||
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
// 接收至少一个命令包。
|
||||
var commandResult = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证停止运动时会发送结束包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StopMotionAsync_SendsEndPacket()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
// 接收并丢弃初始化包。
|
||||
await _server.ReceiveAsync(_cts.Token);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
// 服务器应该收到结束包。
|
||||
var endResult = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.ControlPacketLength, endResult.Buffer.Length);
|
||||
Assert.Equal(Convert.FromHexString("0000000200000001"), endResult.Buffer);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证响应解析和最新响应缓存。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetLatestResponse_ParsesIncomingResponse()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
// 接收初始化包，获取客户端端点。
|
||||
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||
var clientEndpoint = initResult.RemoteEndPoint;
|
||||
|
||||
// 构造 132B 响应包并发送回客户端。
|
||||
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), 5);
|
||||
responsePacket[0x0c] = 15; // 所有状态位为真。
|
||||
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x14, 4), 999u);
|
||||
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x18, 4), 10.0f);
|
||||
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x3c, 4), 0.5f);
|
||||
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x60, 4), 1.0f);
|
||||
|
||||
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
||||
|
||||
// 给接收循环留出时间。
|
||||
await Task.Delay(200, _cts.Token);
|
||||
|
||||
var latest = client.GetLatestResponse();
|
||||
Assert.NotNull(latest);
|
||||
Assert.Equal(5u, latest.Sequence);
|
||||
Assert.True(latest.AcceptsCommand);
|
||||
Assert.True(latest.ReceivedCommand);
|
||||
Assert.True(latest.SystemReady);
|
||||
Assert.True(latest.RobotInMotion);
|
||||
Assert.Equal(999u, latest.Timestamp);
|
||||
Assert.Equal(10.0, latest.Pose[0], precision: 6);
|
||||
Assert.Equal(0.5, latest.JointDegrees[0], precision: 6);
|
||||
Assert.Equal(1.0, latest.MotorCurrents[0], precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task UpdateCommand_ReplacesCurrentCommand()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
|
||||
client.UpdateCommand(command1);
|
||||
client.StartMotion();
|
||||
|
||||
var result1 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
client.UpdateCommand(command2);
|
||||
|
||||
var result2 = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StartMotion_BeforeConnect_Throws()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
Assert.Throws<InvalidOperationException>(() => client.StartMotion());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_MaintainsSubMillisecondPeriod()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await _server.ReceiveAsync(_cts.Token); // init
|
||||
|
||||
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
// 收集 5 个命令包到达时间戳。
|
||||
var timestamps = new List<DateTimeOffset>();
|
||||
for (var i = 0; i < 5; i++)
|
||||
{
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
timestamps.Add(DateTimeOffset.UtcNow);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
// 计算相邻包间隔并断言最大抖动。
|
||||
var intervals = new List<TimeSpan>();
|
||||
for (var i = 1; i < timestamps.Count; i++)
|
||||
{
|
||||
intervals.Add(timestamps[i] - timestamps[i - 1]);
|
||||
}
|
||||
|
||||
// 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
|
||||
Assert.All(intervals, interval =>
|
||||
{
|
||||
Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
});
|
||||
}
|
||||
}
|
||||
220
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
Normal file
220
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
Normal file
@@ -0,0 +1,220 @@
|
||||
using System.Buffers.Binary;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC 真机三条通信链路的二进制协议基础与逆向抓包样本一致。
|
||||
/// </summary>
|
||||
public sealed class FanucProtocolTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 程序命令封包与抓包中的 StopProg("RVBUSTSM") 完全一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_PacksCapturedStopProgramFrame()
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM");
|
||||
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
|
||||
frame);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 短响应和程序状态响应可以按抓包字段解析。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_ParsesCapturedResponses()
|
||||
{
|
||||
var stopResponse = FanucCommandProtocol.ParseResultResponse(
|
||||
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
|
||||
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
|
||||
Convert.FromHexString("646f7a000000160000200300000000000000017a6f64"));
|
||||
|
||||
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
|
||||
Assert.True(stopResponse.IsSuccess);
|
||||
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, statusResponse.MessageId);
|
||||
Assert.True(statusResponse.IsSuccess);
|
||||
Assert.Equal(1u, statusResponse.ProgramStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 的速度倍率、TCP 和 IO 请求体字段顺序与逆向文档一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_PacksParameterCommandBodies()
|
||||
{
|
||||
var setTcpFrame = FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
|
||||
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000000e000022067a6f64"),
|
||||
FanucCommandProtocol.PackGetSpeedRatioCommand());
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002207000000507a6f64"),
|
||||
FanucCommandProtocol.PackSetSpeedRatioCommand(0.8));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002200000000017a6f64"),
|
||||
FanucCommandProtocol.PackGetTcpCommand(1));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a000000160000220800000002000000077a6f64"),
|
||||
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001a0000220900000002000000073f8000007a6f64"),
|
||||
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true));
|
||||
Assert.Equal(FanucCommandMessageIds.SetTcp, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(7, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(11, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(15, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(39, 4)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 参数响应解析使用各自不同的字段顺序。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_ParsesParameterResponses()
|
||||
{
|
||||
var speedRatioResponse = FanucCommandProtocol.ParseSpeedRatioResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetSpeedRatio,
|
||||
Convert.FromHexString("0000005000000000")));
|
||||
var tcpResponse = FanucCommandProtocol.ParseTcpResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetTcp,
|
||||
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")));
|
||||
var ioResponse = FanucCommandProtocol.ParseIoResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetIo,
|
||||
Convert.FromHexString("000000003f800000")));
|
||||
|
||||
Assert.True(speedRatioResponse.IsSuccess);
|
||||
Assert.Equal(0.8, speedRatioResponse.Ratio, precision: 6);
|
||||
Assert.True(tcpResponse.IsSuccess);
|
||||
Assert.Equal(1u, tcpResponse.TcpId);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], tcpResponse.Pose);
|
||||
Assert.True(ioResponse.IsSuccess);
|
||||
Assert.True(ioResponse.Value);
|
||||
Assert.Equal(1.0, ioResponse.NumericValue, precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StateProtocol_ParsesCapturedStateFrame()
|
||||
{
|
||||
var frame = FanucStateProtocol.ParseFrame(Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64"));
|
||||
|
||||
Assert.Equal(0u, frame.MessageId);
|
||||
Assert.Equal(6, frame.Pose.Count);
|
||||
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
||||
Assert.Equal(frame.Pose, frame.CartesianPose);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
|
||||
Assert.Equal(frame.TailWords, frame.RawTailWords);
|
||||
Assert.Equal(2u, frame.StatusWord0);
|
||||
Assert.Equal(0u, frame.StatusWord1);
|
||||
Assert.Equal(0u, frame.StatusWord2);
|
||||
Assert.Equal(1u, frame.StatusWord3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 pcap 中多条唯一 TCP 10010 状态帧都符合固定 90B 布局。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData("646f7a0000005a0000000040eac85a43b2ef4043aba8e9421ed9c1c2828105c2ed981f3fbdbda0bed4764ebe92aacc3efd9f0a3f317ce9be5d4580000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a00000000415aab64440a5302439adef542b39739c293c441431d50423fcdb7003d862fe3beca5730bf60eab23f148e403f89269d000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a000000004221b6f9440b9ce043a129ac42b292bac29cba78431bddcb3fc743213d90268dbeba5351bf64bc1b3f0cbdf73f826864000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
public void StateProtocol_ParsesMultipleCapturedPcapFrames(string frameHex)
|
||||
{
|
||||
var frameBytes = Convert.FromHexString(frameHex);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(frameBytes);
|
||||
|
||||
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
|
||||
Assert.Equal(6, frame.CartesianPose.Count);
|
||||
Assert.Equal(6, frame.JointDegrees.Count);
|
||||
Assert.Equal(3, frame.ExternalAxes.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧会拒绝损坏的长度和 magic，避免后台循环缓存坏帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StateProtocol_RejectsMalformedStateFrames()
|
||||
{
|
||||
var validFrame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
var wrongMagic = validFrame.ToArray();
|
||||
wrongMagic[0] = 0;
|
||||
var wrongLength = validFrame.ToArray();
|
||||
wrongLength[6] = 0x59;
|
||||
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(validFrame.AsSpan(0, validFrame.Length - 1)));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongMagic));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongLength));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 UDP 60015 的 J519 初始化、结束和命令包字段布局。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void J519Protocol_PacksControlAndCommandPackets()
|
||||
{
|
||||
var command = new FanucJ519Command(
|
||||
sequence: 2,
|
||||
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
|
||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||
|
||||
Assert.Equal(Convert.FromHexString("0000000000000001"), FanucJ519Protocol.PackInitPacket());
|
||||
Assert.Equal(Convert.FromHexString("0000000200000001"), FanucJ519Protocol.PackEndPacket());
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, packet.Length);
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x00, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x04, 4)));
|
||||
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
|
||||
Assert.Equal(2, packet[0x0d]);
|
||||
Assert.Equal(1, packet[0x12]);
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
|
||||
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
|
||||
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void J519Protocol_ParsesResponsePacket()
|
||||
{
|
||||
var packet = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, 4), 0);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, 4), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, 4), 12);
|
||||
packet[0x0c] = 15;
|
||||
packet[0x0d] = 2;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, 2), 1);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, 2), 255);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x12, 2), 10);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x14, 4), 1234);
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x18, 4), 100.5f);
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x3c, 4), 1.25f);
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x60, 4), 2.5f);
|
||||
|
||||
var response = FanucJ519Protocol.ParseResponse(packet);
|
||||
|
||||
Assert.Equal(12u, response.Sequence);
|
||||
Assert.Equal(15, response.Status);
|
||||
Assert.True(response.AcceptsCommand);
|
||||
Assert.True(response.ReceivedCommand);
|
||||
Assert.True(response.SystemReady);
|
||||
Assert.True(response.RobotInMotion);
|
||||
Assert.Equal(10, response.ReadIoValue);
|
||||
Assert.Equal(1234u, response.Timestamp);
|
||||
Assert.Equal(100.5, response.Pose[0], precision: 6);
|
||||
Assert.Equal(1.25, response.JointDegrees[0], precision: 6);
|
||||
Assert.Equal(2.5, response.MotorCurrents[0], precision: 6);
|
||||
}
|
||||
}
|
||||
263
tests/Flyshot.Core.Tests/FanucStateClientTests.cs
Normal file
263
tests/Flyshot.Core.Tests/FanucStateClientTests.cs
Normal file
@@ -0,0 +1,263 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC TCP 10010 状态通道客户端的后台接收与缓存能力。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientTests : IDisposable
|
||||
{
|
||||
private readonly TcpListener _listener;
|
||||
private readonly CancellationTokenSource _cts = new();
|
||||
|
||||
/// <summary>
|
||||
/// 在随机可用端口启动本地模拟控制器。
|
||||
/// </summary>
|
||||
public FanucStateClientTests()
|
||||
{
|
||||
_listener = new TcpListener(IPAddress.Loopback, 0);
|
||||
_listener.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取分配给本地模拟控制器的端口。
|
||||
/// </summary>
|
||||
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
|
||||
|
||||
/// <summary>
|
||||
/// 清理模拟控制器和取消源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
_cts.Cancel();
|
||||
_listener.Stop();
|
||||
_cts.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态客户端可以连接本地模拟控制器。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ConnectAsync_ConnectsToLocalListener()
|
||||
{
|
||||
using var client = new FanucStateClient();
|
||||
var acceptTask = _listener.AcceptTcpClientAsync();
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
Assert.True(client.IsConnected);
|
||||
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证后台循环能正确解析抓包样本状态帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetLatestFrame_ReceivesAndParsesCapturedStateFrame()
|
||||
{
|
||||
using var client = new FanucStateClient();
|
||||
var capturedFrame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
|
||||
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
// 给后台循环留出接收和解析的时间。
|
||||
await Task.Delay(200, _cts.Token);
|
||||
|
||||
var latest = client.GetLatestFrame();
|
||||
Assert.NotNull(latest);
|
||||
Assert.Equal(0u, latest.MessageId);
|
||||
Assert.Equal(6, latest.Pose.Count);
|
||||
Assert.Equal(9, latest.JointOrExtensionValues.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], latest.TailWords);
|
||||
|
||||
client.Disconnect();
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态客户端在连接前调用 GetLatestFrame 返回 null。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void GetLatestFrame_BeforeConnect_ReturnsNull()
|
||||
{
|
||||
using var client = new FanucStateClient();
|
||||
Assert.Null(client.GetLatestFrame());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 Disconnect 后最新帧被清空。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task Disconnect_ClearsLatestFrame()
|
||||
{
|
||||
using var client = new FanucStateClient();
|
||||
var capturedFrame = CapturedStateFrame();
|
||||
|
||||
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
await Task.Delay(200, _cts.Token);
|
||||
Assert.NotNull(client.GetLatestFrame());
|
||||
|
||||
client.Disconnect();
|
||||
Assert.Null(client.GetLatestFrame());
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态通道长时间收不到完整帧时会标记陈旧并触发重连。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatus_MarksFrameStaleAndReconnectsWhenFrameTimesOut()
|
||||
{
|
||||
using var client = new FanucStateClient(new FanucStateClientOptions
|
||||
{
|
||||
FrameTimeout = TimeSpan.FromMilliseconds(100),
|
||||
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
|
||||
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
|
||||
ConnectTimeout = TimeSpan.FromSeconds(1),
|
||||
});
|
||||
|
||||
var acceptTask = _listener.AcceptTcpClientAsync(_cts.Token);
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
using var controller = await acceptTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
|
||||
await WaitUntilAsync(
|
||||
() => client.GetStatus().ReconnectAttemptCount > 0,
|
||||
TimeSpan.FromSeconds(2),
|
||||
_cts.Token);
|
||||
|
||||
var status = client.GetStatus();
|
||||
Assert.True(status.IsFrameStale);
|
||||
Assert.True(status.State is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting or FanucStateConnectionState.Connected);
|
||||
Assert.NotNull(status.LastErrorMessage);
|
||||
Assert.Contains("超时", status.LastErrorMessage);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态通道在控制柜主动断开后可以退避重连并接收新连接上的状态帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ReceiveLoop_ReconnectsAfterEofAndKeepsReceivingFrames()
|
||||
{
|
||||
using var client = new FanucStateClient(new FanucStateClientOptions
|
||||
{
|
||||
FrameTimeout = TimeSpan.FromMilliseconds(500),
|
||||
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
|
||||
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
|
||||
ConnectTimeout = TimeSpan.FromSeconds(1),
|
||||
});
|
||||
|
||||
var firstFrame = CapturedStateFrame(1);
|
||||
var secondFrame = CapturedStateFrame(2);
|
||||
var handlerTask = RunReconnectControllerAsync(firstFrame, secondFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
await WaitUntilAsync(
|
||||
() => client.GetLatestFrame()?.MessageId == 2u,
|
||||
TimeSpan.FromSeconds(2),
|
||||
_cts.Token);
|
||||
|
||||
var status = client.GetStatus();
|
||||
Assert.Equal(FanucStateConnectionState.Connected, status.State);
|
||||
Assert.True(status.ReconnectAttemptCount >= 1);
|
||||
|
||||
client.Disconnect();
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动模拟控制器，持续发送状态帧流。
|
||||
/// </summary>
|
||||
private async Task RunStreamingControllerAsync(byte[] frames, CancellationToken cancellationToken)
|
||||
{
|
||||
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||
await using var stream = controller.GetStream();
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await stream.WriteAsync(frames, cancellationToken);
|
||||
await Task.Delay(50, cancellationToken);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消。
|
||||
}
|
||||
catch (IOException)
|
||||
{
|
||||
// 客户端断开。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动模拟控制器：第一条连接发一帧后主动断开，第二条连接持续发送新帧。
|
||||
/// </summary>
|
||||
private async Task RunReconnectControllerAsync(byte[] firstFrame, byte[] secondFrame, CancellationToken cancellationToken)
|
||||
{
|
||||
using (var firstController = await _listener.AcceptTcpClientAsync(cancellationToken))
|
||||
{
|
||||
await using var firstStream = firstController.GetStream();
|
||||
await firstStream.WriteAsync(firstFrame, cancellationToken);
|
||||
}
|
||||
|
||||
using var secondController = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||
await using var secondStream = secondController.GetStream();
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await secondStream.WriteAsync(secondFrame, cancellationToken);
|
||||
await Task.Delay(50, cancellationToken);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消。
|
||||
}
|
||||
catch (IOException)
|
||||
{
|
||||
// 客户端断开。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造来自 j519 抓包的状态帧，并按测试需要覆写 message_id。
|
||||
/// </summary>
|
||||
private static byte[] CapturedStateFrame(uint messageId = 0)
|
||||
{
|
||||
var frame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, 4), messageId);
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待异步后台循环达到预期状态，超时后让测试明确失败。
|
||||
/// </summary>
|
||||
private static async Task WaitUntilAsync(Func<bool> predicate, TimeSpan timeout, CancellationToken cancellationToken)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow + timeout;
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
if (predicate())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
await Task.Delay(20, cancellationToken);
|
||||
}
|
||||
|
||||
Assert.True(predicate(), "等待状态通道后台循环达到预期状态超时。");
|
||||
}
|
||||
}
|
||||
@@ -23,10 +23,12 @@
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\src\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -34,6 +34,30 @@ public sealed class PlanningCompatibilityTests
|
||||
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通 ICSP 在最终最优解仍超限时会显式失败，而不是返回不可执行轨迹。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
|
||||
{
|
||||
var request = new TrajectoryRequest(
|
||||
robot: CreateRobotProfile([0.1], [0.1], [0.1]),
|
||||
program: CreateProgram(
|
||||
new[]
|
||||
{
|
||||
new[] { 0.0 },
|
||||
new[] { 10.0 },
|
||||
new[] { 20.0 },
|
||||
new[] { 30.0 }
|
||||
}),
|
||||
method: PlanningMethod.Icsp);
|
||||
|
||||
var planner = new ICspPlanner(maxIterations: 0);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
|
||||
Assert.Contains("global_scale", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
|
||||
/// </summary>
|
||||
|
||||
539
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
Normal file
539
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
Normal file
@@ -0,0 +1,539 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证最小运行时编排链路会把规划结果交给控制器运行时，而不是停留在兼容层内存状态。
|
||||
/// </summary>
|
||||
public sealed class RuntimeOrchestrationTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证 FANUC 最小运行时执行轨迹后会更新状态快照与最终关节位置。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
|
||||
{
|
||||
var runtime = new FanucControllerRuntime();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
runtime.SetActiveController(sim: true);
|
||||
runtime.Connect("192.168.10.101");
|
||||
runtime.EnableRobot(bufferSize: 2);
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(1.2),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
Assert.Equal("Connected", snapshot.ConnectionState);
|
||||
Assert.False(snapshot.IsInMotion);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(FanucStateConnectionState.Connected, false, "Connected")]
|
||||
[InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
|
||||
[InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
|
||||
[InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
|
||||
[InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
|
||||
public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
string expected)
|
||||
{
|
||||
var status = new FanucStateClientStatus(
|
||||
state,
|
||||
isFrameStale,
|
||||
lastFrameAt: null,
|
||||
reconnectAttemptCount: 0,
|
||||
lastErrorMessage: null);
|
||||
|
||||
var actual = FanucControllerRuntime.ResolveRealConnectionState(status);
|
||||
|
||||
Assert.Equal(expected, actual);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(FanucStateConnectionState.Connected, false, true)]
|
||||
[InlineData(FanucStateConnectionState.Connected, true, false)]
|
||||
[InlineData(FanucStateConnectionState.Reconnecting, false, false)]
|
||||
[InlineData(FanucStateConnectionState.TimedOut, false, false)]
|
||||
[InlineData(FanucStateConnectionState.Disconnected, false, false)]
|
||||
public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
bool expected)
|
||||
{
|
||||
var status = new FanucStateClientStatus(
|
||||
state,
|
||||
isFrameStale,
|
||||
lastFrameAt: null,
|
||||
reconnectAttemptCount: 0,
|
||||
lastErrorMessage: null);
|
||||
|
||||
var actual = FanucControllerRuntime.ShouldUseStateFrame(status);
|
||||
|
||||
Assert.Equal(expected, actual);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
void Act() =>
|
||||
orchestrator.PlanOrdinaryTrajectory(
|
||||
robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]);
|
||||
|
||||
Assert.Throws<ArgumentException>(Act);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证已上传飞拍轨迹会经过 self-adapt-icsp 并生成拍照触发时间轴。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
|
||||
Assert.True(bundle.Result.IsValid);
|
||||
Assert.Single(bundle.Result.ShotEvents);
|
||||
Assert.Single(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
var settings = new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: [7, 8],
|
||||
ioKeepCycles: 4,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
|
||||
|
||||
var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
|
||||
Assert.Equal(4, doEvent.HoldCycles);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
var settings = new CompatibilityRobotSettings(
|
||||
useDo: false,
|
||||
ioAddresses: [7, 8],
|
||||
ioKeepCycles: 4,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
|
||||
|
||||
Assert.Single(bundle.Result.ShotEvents);
|
||||
Assert.Empty(bundle.Result.TriggerTimeline);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通轨迹规划后会生成稠密关节采样序列。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
|
||||
var bundle = orchestrator.PlanOrdinaryTrajectory(
|
||||
robot,
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]);
|
||||
|
||||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||||
Assert.NotEmpty(bundle.Result.DenseJointTrajectory);
|
||||
|
||||
// 验证时间单调递增。
|
||||
var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray();
|
||||
for (var i = 1; i < times.Length; i++)
|
||||
{
|
||||
Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。");
|
||||
}
|
||||
|
||||
// 验证每行包含时间 + 6 个关节值。
|
||||
Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||||
|
||||
Assert.NotNull(bundle.Result.DenseJointTrajectory);
|
||||
Assert.True(bundle.Result.DenseJointTrajectory.Count > 1);
|
||||
|
||||
// 采样周期应为 8ms（伺服周期）。
|
||||
var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
|
||||
Assert.Equal(0.008, firstDt, precision: 3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
|
||||
{
|
||||
var service = TestRobotFactory.CreateCompatService();
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: true);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
|
||||
void Act() =>
|
||||
service.ExecuteTrajectory(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
]);
|
||||
|
||||
Assert.Throws<ArgumentException>(Act);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
|
||||
{
|
||||
var tempRoot = CreateTempWorkspaceRoot();
|
||||
try
|
||||
{
|
||||
File.WriteAllText(
|
||||
Path.Combine(tempRoot, "RobotConfig.json"),
|
||||
"""
|
||||
{
|
||||
"robot": {
|
||||
"use_do": true,
|
||||
"io_addr": [7, 8],
|
||||
"io_keep_cycles": 4,
|
||||
"acc_limit": 0.5,
|
||||
"jerk_limit": 0.25,
|
||||
"adapt_icsp_try_num": 3
|
||||
},
|
||||
"flying_shots": {}
|
||||
}
|
||||
""");
|
||||
|
||||
var options = new ControllerClientCompatOptions { WorkspaceRoot = tempRoot };
|
||||
var runtime = new RecordingControllerRuntime();
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader(),
|
||||
new InMemoryFlyshotTrajectoryStore());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
|
||||
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
|
||||
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
|
||||
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(tempRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时工作区。
|
||||
/// </summary>
|
||||
private static string CreateTempWorkspaceRoot()
|
||||
{
|
||||
var tempRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"));
|
||||
var modelDir = Path.Combine(tempRoot, "FlyingShot", "FlyingShot", "Models");
|
||||
Directory.CreateDirectory(modelDir);
|
||||
|
||||
var sourceModel = Path.Combine(
|
||||
TestRobotFactory.GetWorkspaceRoot(),
|
||||
"FlyingShot",
|
||||
"FlyingShot",
|
||||
"Models",
|
||||
"LR_Mate_200iD_7L.robot");
|
||||
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
|
||||
|
||||
return tempRoot;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为运行时编排测试构造稳定的最小领域对象。
|
||||
/// </summary>
|
||||
internal static class TestRobotFactory
|
||||
{
|
||||
/// <summary>
|
||||
/// 构造六轴测试机器人配置，避免运行时测试依赖真实 .robot 文件。
|
||||
/// </summary>
|
||||
/// <returns>可用于规划和运行时状态校验的机器人配置。</returns>
|
||||
public static RobotProfile CreateRobotProfile()
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "TestRobot",
|
||||
modelPath: "Models/Test.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits: Enumerable.Range(1, 6)
|
||||
.Select(static index => new JointLimit($"J{index}", 10.0, 20.0, 100.0))
|
||||
.ToArray(),
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造一条含单个拍照点的上传飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <returns>可用于触发时间轴测试的上传轨迹。</returns>
|
||||
public static ControllerClientCompatUploadedTrajectory CreateUploadedTrajectoryWithSingleShot()
|
||||
{
|
||||
return new ControllerClientCompatUploadedTrajectory(
|
||||
name: "demo-flyshot",
|
||||
waypoints:
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
shotFlags: [false, true, false, false],
|
||||
offsetValues: [0, 1, 0, 0],
|
||||
addressGroups:
|
||||
[
|
||||
Array.Empty<int>(),
|
||||
[7, 8],
|
||||
Array.Empty<int>(),
|
||||
Array.Empty<int>()
|
||||
]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造一份真实依赖注入等价的兼容服务，覆盖运行时和编排器协作。
|
||||
/// </summary>
|
||||
/// <returns>可执行 ControllerClient 兼容语义的服务实例。</returns>
|
||||
public static ControllerClientCompatService CreateCompatService()
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
WorkspaceRoot = GetWorkspaceRoot()
|
||||
};
|
||||
|
||||
return new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
new FanucControllerRuntime(),
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader(),
|
||||
new InMemoryFlyshotTrajectoryStore());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位父工作区根目录，供兼容服务加载真实机器人模型。
|
||||
/// </summary>
|
||||
/// <returns>父工作区根目录。</returns>
|
||||
public 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.");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 内存中的轨迹存储实现，用于避免单元测试污染真实文件系统。
|
||||
/// </summary>
|
||||
internal sealed class InMemoryFlyshotTrajectoryStore : IFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _store = new(StringComparer.Ordinal);
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
_store[trajectory.Name] = trajectory;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
_store.Remove(trajectoryName);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
settings = null;
|
||||
return _store;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。
|
||||
/// </summary>
|
||||
internal sealed class RecordingControllerRuntime : IControllerRuntime
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取最近一次 ResetRobot 收到的机器人配置。
|
||||
/// </summary>
|
||||
public RobotProfile? LastRobotProfile { get; private set; }
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
LastRobotProfile = robot;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public double GetSpeedRatio() => 1.0;
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetSpeedRatio(double ratio)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetTcp(double x, double y, double z)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool GetIo(int port, string ioType) => false;
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetIo(int port, bool value, string ioType)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetJointPositions() => Array.Empty<double>();
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||||
|
||||
/// <inheritdoc />
|
||||
public ControllerStateSnapshot GetSnapshot()
|
||||
{
|
||||
return new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.UtcNow,
|
||||
connectionState: "Connected",
|
||||
isEnabled: true,
|
||||
isInMotion: false,
|
||||
speedRatio: 1.0,
|
||||
jointPositions: Array.Empty<double>(),
|
||||
cartesianPose: Array.Empty<double>(),
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定宿主当前应注册 HTTP-only 的 ControllerClient 兼容服务，而不是旧 TCP 网关入口。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatRegistrationTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证宿主能从 DI 中解析新的兼容服务。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void Host_Registers_ControllerClientCompat_Service()
|
||||
{
|
||||
var service = factory.Services.GetService<IControllerClientCompatService>();
|
||||
|
||||
Assert.NotNull(service);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
using System.Net;
|
||||
using Microsoft.AspNetCore.Mvc.Testing;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 `/debug` 在线 API 调试页的暴露策略与基础内容契约。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 调试页与 Swagger UI 共用 <c>Swagger:Enabled</c> 开关，开关关闭时
|
||||
/// 调试页与 `/swagger` 一同下线，避免生产环境意外暴露调试入口。
|
||||
/// </remarks>
|
||||
public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
private readonly FlyshotServerFactory _factory = factory;
|
||||
|
||||
/// <summary>
|
||||
/// 当 Swagger 启用时，`/debug` 应当返回完整的 HTML 调试页。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebug_WhenSwaggerEnabled_ReturnsConsoleHtml()
|
||||
{
|
||||
// 默认配置即开启 Swagger，调试页应当作为浏览器可直接打开的 HTML 暴露。
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/debug");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
Assert.StartsWith("text/html", response.Content.Headers.ContentType?.MediaType);
|
||||
|
||||
var html = await response.Content.ReadAsStringAsync();
|
||||
|
||||
// 页面标题与稳定锚点用于回归保护：调试页骨架一旦丢失，测试立即报警。
|
||||
Assert.Contains("Flyshot Replacement 接口调试", html, StringComparison.Ordinal);
|
||||
Assert.Contains("id=\"debug-console-app\"", html, StringComparison.Ordinal);
|
||||
|
||||
// 控制器需要在返回 HTML 前把 Swagger JSON URL 注入到页面占位符里，
|
||||
// 否则前端无法在加载时拉取 OpenAPI 文档。
|
||||
Assert.Contains("/swagger/v1/swagger.json", html, StringComparison.Ordinal);
|
||||
Assert.DoesNotContain("__SWAGGER_JSON_URL__", html, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当 Swagger 关闭时，`/debug` 应当与 `/swagger` 同步下线（404）。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebug_WhenSwaggerDisabled_ReturnsNotFound()
|
||||
{
|
||||
// 显式把 Swagger:Enabled 置为 false，此时调试页也不应当被访问到。
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(false);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/debug");
|
||||
|
||||
Assert.Equal(HttpStatusCode.NotFound, response.StatusCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 调试页需要从 Swagger JSON 中读取所有端点，因此 Swagger 文档必须包含基础和兼容层的代表性路由。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SwaggerDocument_ContainsRepresentativeRoutesForDebugConsole()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/swagger/v1/swagger.json");
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
|
||||
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||
using var document = await System.Text.Json.JsonDocument.ParseAsync(responseStream);
|
||||
var paths = document.RootElement.GetProperty("paths");
|
||||
|
||||
// 这些路径分别覆盖：基础探活、状态快照、版本查询、上传飞拍轨迹四种典型形态。
|
||||
AssertPathExists(paths, "/healthz");
|
||||
AssertPathExists(paths, "/api/status/snapshot");
|
||||
AssertPathExists(paths, "/get_server_version");
|
||||
AssertPathExists(paths, "/upload_flyshot");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 状态页应当提供跳转到调试页的入口，便于现场顺手跳转。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatusPage_LinksToDebugConsole()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/status");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
var html = await response.Content.ReadAsStringAsync();
|
||||
|
||||
// 状态页头部需要至少一个指向 `/debug` 的链接，文案不强制以保留排版调整空间。
|
||||
Assert.Contains("href=\"/debug\"", html, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查 Swagger 文档中是否存在指定路径，兼容尾斜杠归一化两种形态。
|
||||
/// </summary>
|
||||
/// <param name="paths">OpenAPI 文档中的 paths 节点。</param>
|
||||
/// <param name="route">期望存在的路由字符串。</param>
|
||||
private static void AssertPathExists(System.Text.Json.JsonElement paths, string route)
|
||||
{
|
||||
// OpenAPI 生成器会把部分尾斜杠路径规范化，这里同时接受两种形态。
|
||||
var withSlash = route.EndsWith('/') ? route : route + "/";
|
||||
var withoutSlash = route.EndsWith('/') ? route.TrimEnd('/') : route;
|
||||
|
||||
Assert.True(
|
||||
paths.TryGetProperty(withSlash, out _) || paths.TryGetProperty(withoutSlash, out _),
|
||||
$"Swagger 文档应当包含路径 {route}");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造覆盖了 <c>Swagger:Enabled</c> 配置项的测试宿主工厂。
|
||||
/// </summary>
|
||||
/// <param name="enabled">期望的 Swagger 启用状态。</param>
|
||||
/// <returns>已应用配置覆盖的测试工厂。</returns>
|
||||
private WebApplicationFactory<Program> CreateFactoryWithSwaggerEnabled(bool enabled)
|
||||
{
|
||||
return _factory.WithWebHostBuilder(builder =>
|
||||
{
|
||||
builder.ConfigureAppConfiguration((_, configurationBuilder) =>
|
||||
{
|
||||
// 通过 InMemory 配置覆盖 appsettings.json 中的 Swagger 开关，避免修改磁盘文件。
|
||||
configurationBuilder.AddInMemoryCollection(new Dictionary<string, string?>
|
||||
{
|
||||
["Swagger:Enabled"] = enabled ? "true" : "false",
|
||||
["Swagger:DocumentName"] = "v1",
|
||||
["Swagger:Title"] = "Flyshot Replacement HTTP API",
|
||||
["Swagger:Version"] = "v1",
|
||||
["Swagger:JsonRouteTemplate"] = "swagger/{documentName}/swagger.json",
|
||||
["Swagger:RoutePrefix"] = "swagger"
|
||||
});
|
||||
});
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -24,6 +24,7 @@
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\src\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj" />
|
||||
<ProjectReference Include="..\..\src\Flyshot.Server.Host\Flyshot.Server.Host.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
using System.Net;
|
||||
using System.Text.Json;
|
||||
using Microsoft.AspNetCore.Mvc.Testing;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定标准 MVC 宿主需要提供的 Swagger 与 CORS 行为，避免后续回退成只够跑通的最小配置。
|
||||
/// </summary>
|
||||
public sealed class HostMvcConfigurationTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证宿主会公开标准 Swagger JSON，并且文档标题和旧 HTTP 兼容路径都能从配置和控制器路由中导出。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SwaggerDocument_ExposesConfiguredMetadataAndLegacyRoutes()
|
||||
{
|
||||
using var configuredFactory = CreateConfiguredFactory(factory);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/swagger/v1/swagger.json");
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
|
||||
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||
using var document = await JsonDocument.ParseAsync(responseStream);
|
||||
var root = document.RootElement;
|
||||
var paths = root.GetProperty("paths");
|
||||
|
||||
Assert.Equal("3.0.1", root.GetProperty("openapi").GetString());
|
||||
Assert.Equal("Flyshot Replacement HTTP API", root.GetProperty("info").GetProperty("title").GetString());
|
||||
// OpenAPI 文档会把部分带尾斜杠的路由规范化为无尾斜杠形式，这里同时接受两种键。
|
||||
Assert.True(paths.TryGetProperty("/robot_info/", out _) || paths.TryGetProperty("/robot_info", out _));
|
||||
Assert.True(paths.TryGetProperty("/healthz", out _) || paths.TryGetProperty("/healthz/", out _));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证宿主会按配置对旧 HTTP API 路由返回标准 CORS 预检响应。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task CorsPreflight_ReturnsConfiguredAllowOriginHeaders()
|
||||
{
|
||||
using var configuredFactory = CreateConfiguredFactory(factory);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var request = new HttpRequestMessage(HttpMethod.Options, "/robot_info/");
|
||||
request.Headers.Add("Origin", "http://localhost:3000");
|
||||
request.Headers.Add("Access-Control-Request-Method", "GET");
|
||||
request.Headers.Add("Access-Control-Request-Headers", "content-type");
|
||||
|
||||
using var response = await client.SendAsync(request);
|
||||
Assert.Equal(HttpStatusCode.NoContent, response.StatusCode);
|
||||
Assert.True(response.Headers.TryGetValues("Access-Control-Allow-Origin", out var allowedOrigins));
|
||||
Assert.Contains("http://localhost:3000", allowedOrigins);
|
||||
Assert.True(response.Headers.TryGetValues("Access-Control-Allow-Methods", out var allowedMethods));
|
||||
Assert.Contains("GET", string.Join(",", allowedMethods));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为测试宿主注入标准 Swagger 与 CORS 配置，避免依赖开发机本地环境。
|
||||
/// </summary>
|
||||
private static WebApplicationFactory<Program> CreateConfiguredFactory(FlyshotServerFactory factory)
|
||||
{
|
||||
return factory.WithWebHostBuilder(builder =>
|
||||
{
|
||||
builder.ConfigureAppConfiguration((_, configurationBuilder) =>
|
||||
{
|
||||
configurationBuilder.AddInMemoryCollection(new Dictionary<string, string?>
|
||||
{
|
||||
["Swagger:Enabled"] = "true",
|
||||
["Swagger:DocumentName"] = "v1",
|
||||
["Swagger:Title"] = "Flyshot Replacement HTTP API",
|
||||
["Swagger:Version"] = "v1",
|
||||
["Cors:PolicyName"] = "LegacyHttpApi",
|
||||
["Cors:AllowedOrigins:0"] = "http://localhost:3000",
|
||||
["Cors:AllowedMethods:0"] = "GET",
|
||||
["Cors:AllowedMethods:1"] = "POST",
|
||||
["Cors:AllowedMethods:2"] = "OPTIONS",
|
||||
["Cors:AllowedHeaders:0"] = "content-type"
|
||||
});
|
||||
});
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,289 @@
|
||||
using System.Net;
|
||||
using System.Net.Http.Json;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定 flyshot-uaes-interface 现有 FastAPI 层的 HTTP 路径、参数绑定和返回 JSON 外形。
|
||||
/// </summary>
|
||||
public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证根路径会返回旧 HTTP 服务使用的 Hello World JSON，而不是跳转到健康检查页。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task Root_ReturnsLegacyHelloWorldPayload()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/");
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
|
||||
using var json = await ReadJsonAsync(response);
|
||||
Assert.Equal("Hello World", json.RootElement.GetProperty("message").GetString());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证初始化链路和机器人信息接口会保持旧 FastAPI 服务的路径与返回字段风格。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task InitEndpoints_ExposeLegacyRobotInfoAndSpeedRatioShape()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
|
||||
using (var connectServerResponse = await client.PostAsync("/connect_server/?server_ip=127.0.0.1&port=50001", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, connectServerResponse.StatusCode);
|
||||
using var connectServerJson = await ReadJsonAsync(connectServerResponse);
|
||||
Assert.Equal("connected", connectServerJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var setupResponse = await client.PostAsync("/setup_robot/?robot_name=FANUC_LR_Mate_200iD", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, setupResponse.StatusCode);
|
||||
using var setupJson = await ReadJsonAsync(setupResponse);
|
||||
Assert.Equal("robot setup", setupJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var isSetupResponse = await client.GetAsync("/is_setup/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, isSetupResponse.StatusCode);
|
||||
using var isSetupJson = await ReadJsonAsync(isSetupResponse);
|
||||
Assert.True(isSetupJson.RootElement.GetProperty("is_setup").GetBoolean());
|
||||
}
|
||||
|
||||
using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=true", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, activeControllerResponse.StatusCode);
|
||||
using var activeControllerJson = await ReadJsonAsync(activeControllerResponse);
|
||||
Assert.Equal("active controller set", activeControllerJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var connectRobotResponse = await client.PostAsync("/connect_robot/?ip=192.168.10.101", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, connectRobotResponse.StatusCode);
|
||||
using var connectRobotJson = await ReadJsonAsync(connectRobotResponse);
|
||||
Assert.Equal("robot connected", connectRobotJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var enableRobotResponse = await client.GetAsync("/enable_robot/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, enableRobotResponse.StatusCode);
|
||||
using var enableRobotJson = await ReadJsonAsync(enableRobotResponse);
|
||||
Assert.True(enableRobotJson.RootElement.GetProperty("enable_robot").GetBoolean());
|
||||
}
|
||||
|
||||
using (var robotInfoResponse = await client.GetAsync("/robot_info/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, robotInfoResponse.StatusCode);
|
||||
using var robotInfoJson = await ReadJsonAsync(robotInfoResponse);
|
||||
var robotInfoRoot = robotInfoJson.RootElement;
|
||||
|
||||
Assert.Equal("FANUC_LR_Mate_200iD", robotInfoRoot.GetProperty("name").GetString());
|
||||
Assert.Equal("flyshot-replacement-controller-client-compat/0.1.0", robotInfoRoot.GetProperty("server_version").GetString());
|
||||
Assert.Equal(6, robotInfoRoot.GetProperty("dof").GetInt32());
|
||||
Assert.Equal(1.0, robotInfoRoot.GetProperty("speed_ratio").GetDouble(), precision: 6);
|
||||
}
|
||||
|
||||
using (var setSpeedRatioResponse = await client.PostAsJsonAsync("/set_speedRatio/", new { speed = 0.8 }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, setSpeedRatioResponse.StatusCode);
|
||||
using var setSpeedRatioJson = await ReadJsonAsync(setSpeedRatioResponse);
|
||||
Assert.Equal("set_speedRatio executed", setSpeedRatioJson.RootElement.GetProperty("message").GetString());
|
||||
Assert.Equal(0, setSpeedRatioJson.RootElement.GetProperty("returnCode").GetInt32());
|
||||
}
|
||||
|
||||
using var updatedRobotInfoResponse = await client.GetAsync("/robot_info/");
|
||||
Assert.Equal(HttpStatusCode.OK, updatedRobotInfoResponse.StatusCode);
|
||||
using var updatedRobotInfoJson = await ReadJsonAsync(updatedRobotInfoResponse);
|
||||
Assert.Equal(0.8, updatedRobotInfoJson.RootElement.GetProperty("speed_ratio").GetDouble(), precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP、关节位置和位姿相关 HTTP 接口会保持旧服务的请求体与响应体结构。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task MotionStateEndpoints_RoundTripLegacyPayloadShapes()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
await InitializeRobotAsync(client);
|
||||
|
||||
using (var setTcpResponse = await client.PostAsJsonAsync("/set_tcp/", new { x = 1.0, y = 2.0, z = 3.0 }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, setTcpResponse.StatusCode);
|
||||
using var setTcpJson = await ReadJsonAsync(setTcpResponse);
|
||||
Assert.Equal("TCP set", setTcpJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var getTcpResponse = await client.GetAsync("/get_tcp/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, getTcpResponse.StatusCode);
|
||||
using var getTcpJson = await ReadJsonAsync(getTcpResponse);
|
||||
var tcpValues = getTcpJson.RootElement.GetProperty("tcp").EnumerateArray().Select(static value => value.GetDouble()).ToArray();
|
||||
Assert.Equal([1.0, 2.0, 3.0], tcpValues);
|
||||
}
|
||||
|
||||
using (var moveJointResponse = await client.PostAsJsonAsync("/move_joint/", new { joints = new[] { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 } }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, moveJointResponse.StatusCode);
|
||||
using var moveJointJson = await ReadJsonAsync(moveJointResponse);
|
||||
Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode);
|
||||
using var getJointPositionJson = await ReadJsonAsync(getJointPositionResponse);
|
||||
var root = getJointPositionJson.RootElement;
|
||||
Assert.True(root.GetProperty("success").GetBoolean());
|
||||
var jointValues = root.GetProperty("points").EnumerateArray().Select(static value => value.GetDouble()).ToArray();
|
||||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], jointValues);
|
||||
}
|
||||
|
||||
using var getPoseResponse = await client.GetAsync("/get_pose");
|
||||
Assert.Equal(HttpStatusCode.OK, getPoseResponse.StatusCode);
|
||||
using var getPoseJson = await ReadJsonAsync(getPoseResponse);
|
||||
Assert.Equal(7, getPoseJson.RootElement.GetProperty("pose").GetArrayLength());
|
||||
|
||||
using (var executeTrajectoryResponse = await client.PostAsJsonAsync("/execute_trajectory/", new
|
||||
{
|
||||
method = "icsp",
|
||||
save_traj = true,
|
||||
waypoints = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.1, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.2, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.3, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
}
|
||||
}))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, executeTrajectoryResponse.StatusCode);
|
||||
using var executeTrajectoryJson = await ReadJsonAsync(executeTrajectoryResponse);
|
||||
Assert.Equal("trajectory executed", executeTrajectoryJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task FlyshotEndpoints_RoundTripLegacyUploadExecuteAndDeleteFlow()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
await InitializeRobotAsync(client);
|
||||
|
||||
var uploadPayload = new
|
||||
{
|
||||
addrs = new[]
|
||||
{
|
||||
new[] { 7, 8 },
|
||||
new[] { 7, 8 },
|
||||
Array.Empty<int>(),
|
||||
Array.Empty<int>()
|
||||
},
|
||||
name = "demo-http-flyshot",
|
||||
offset_values = new[] { 0.0, 1.0, 0.0, 0.0 },
|
||||
shot_flags = new[] { false, true, false, false },
|
||||
waypoints = new[]
|
||||
{
|
||||
new[] { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
||||
new[] { 0.2, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
||||
new[] { 0.3, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
||||
new[] { 0.4, 0.2, 0.3, 0.4, 0.5, 0.6 }
|
||||
}
|
||||
};
|
||||
|
||||
using (var uploadResponse = await client.PostAsJsonAsync("/upload_flyshot/", uploadPayload))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, uploadResponse.StatusCode);
|
||||
using var uploadJson = await ReadJsonAsync(uploadResponse);
|
||||
Assert.Equal("FlyShot uploaded", uploadJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
|
||||
using (var listResponse = await client.GetAsync("/list_flyShotTraj/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, listResponse.StatusCode);
|
||||
using var listJson = await ReadJsonAsync(listResponse);
|
||||
var names = listJson.RootElement.GetProperty("flyshot_trajs").EnumerateArray().Select(static value => value.GetString()).ToArray();
|
||||
Assert.Contains("demo-http-flyshot", names);
|
||||
}
|
||||
|
||||
using (var validResponse = await client.PostAsJsonAsync("/is_flyShotTrajValid/", new
|
||||
{
|
||||
name = "demo-http-flyshot",
|
||||
method = "icsp",
|
||||
save_traj = false
|
||||
}))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, validResponse.StatusCode);
|
||||
using var validJson = await ReadJsonAsync(validResponse);
|
||||
Assert.True(validJson.RootElement.GetProperty("valid").GetBoolean());
|
||||
Assert.True(validJson.RootElement.GetProperty("time").GetDouble() > 0.0);
|
||||
}
|
||||
|
||||
using (var executeResponse = await client.PostAsJsonAsync("/execute_flyshot/", new
|
||||
{
|
||||
name = "demo-http-flyshot",
|
||||
move_to_start = true,
|
||||
method = "icsp",
|
||||
save_traj = true,
|
||||
use_cache = true
|
||||
}))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);
|
||||
using var executeJson = await ReadJsonAsync(executeResponse);
|
||||
var executeRoot = executeJson.RootElement;
|
||||
Assert.Equal("FlyShot executed", executeRoot.GetProperty("status").GetString());
|
||||
Assert.True(executeRoot.GetProperty("success").GetBoolean());
|
||||
}
|
||||
|
||||
using (var saveInfoResponse = await client.PostAsJsonAsync("/save_traj_info/", new
|
||||
{
|
||||
name = "demo-http-flyshot",
|
||||
method = "icsp"
|
||||
}))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, saveInfoResponse.StatusCode);
|
||||
using var saveInfoJson = await ReadJsonAsync(saveInfoResponse);
|
||||
Assert.True(saveInfoJson.RootElement.GetProperty("success").GetBoolean());
|
||||
}
|
||||
|
||||
using (var deleteResponse = await client.PostAsJsonAsync("/delete_flyshot/", new { name = "demo-http-flyshot" }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, deleteResponse.StatusCode);
|
||||
using var deleteJson = await ReadJsonAsync(deleteResponse);
|
||||
Assert.Equal("FlyShot deleted", deleteJson.RootElement.GetProperty("status").GetString());
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复用旧 API 层常见的初始化顺序，把当前宿主推进到可执行动作的最小状态。
|
||||
/// </summary>
|
||||
private static async Task InitializeRobotAsync(HttpClient client)
|
||||
{
|
||||
using var initResponse = await client.PostAsJsonAsync("/init_mpc_robt", new
|
||||
{
|
||||
server_ip = "127.0.0.1",
|
||||
port = 50001,
|
||||
robot_name = "FANUC_LR_Mate_200iD",
|
||||
robot_ip = "192.168.10.101",
|
||||
sim = true
|
||||
});
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, initResponse.StatusCode);
|
||||
using var initJson = await ReadJsonAsync(initResponse);
|
||||
Assert.Equal("init_Success", initJson.RootElement.GetProperty("message").GetString());
|
||||
Assert.Equal(0, initJson.RootElement.GetProperty("returnCode").GetInt32());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 统一把 HTTP 响应体解析成 JsonDocument，便于对旧接口的字段形状做精确断言。
|
||||
/// </summary>
|
||||
private static async Task<JsonDocument> ReadJsonAsync(HttpResponseMessage response)
|
||||
{
|
||||
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||
return await JsonDocument.ParseAsync(responseStream);
|
||||
}
|
||||
}
|
||||
91
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
91
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
@@ -0,0 +1,91 @@
|
||||
using System.Net;
|
||||
using System.Net.Http.Json;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态监控页面和状态快照 API 能读取当前 ControllerClient 兼容层状态。
|
||||
/// </summary>
|
||||
public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证状态页返回可由浏览器直接打开的 HTML，并引用状态快照 API。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatusPage_ReturnsMonitoringHtml()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/status");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
Assert.StartsWith("text/html", response.Content.Headers.ContentType?.MediaType);
|
||||
|
||||
var html = await response.Content.ReadAsStringAsync();
|
||||
Assert.Contains("Flyshot Replacement 状态监控", html, StringComparison.Ordinal);
|
||||
Assert.Contains("/api/status/snapshot", html, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatusSnapshot_ReturnsRuntimeStateAfterLegacyInitialization()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
await InitializeRobotAsync(client);
|
||||
|
||||
using (var speedResponse = await client.PostAsJsonAsync("/set_speedRatio/", new { speed = 0.75 }))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, speedResponse.StatusCode);
|
||||
}
|
||||
|
||||
using var response = await client.GetAsync("/api/status/snapshot");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
|
||||
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||
using var json = await JsonDocument.ParseAsync(responseStream);
|
||||
var root = json.RootElement;
|
||||
var snapshot = root.GetProperty("snapshot");
|
||||
|
||||
Assert.Equal("ok", root.GetProperty("status").GetString());
|
||||
Assert.True(root.GetProperty("isSetup").GetBoolean());
|
||||
Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString());
|
||||
Assert.Equal(6, root.GetProperty("degreesOfFreedom").GetInt32());
|
||||
Assert.Empty(root.GetProperty("uploadedTrajectories").EnumerateArray());
|
||||
Assert.Equal("Connected", snapshot.GetProperty("connectionState").GetString());
|
||||
Assert.True(snapshot.GetProperty("isEnabled").GetBoolean());
|
||||
Assert.False(snapshot.GetProperty("isInMotion").GetBoolean());
|
||||
Assert.Equal(0.75, snapshot.GetProperty("speedRatio").GetDouble(), precision: 6);
|
||||
Assert.Equal(6, snapshot.GetProperty("jointPositions").GetArrayLength());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 初始化旧 HTTP 兼容链路，使状态页可以读取一个完整的已连接状态。
|
||||
/// </summary>
|
||||
/// <param name="client">测试 HTTP 客户端。</param>
|
||||
private static async Task InitializeRobotAsync(HttpClient client)
|
||||
{
|
||||
using (var setupResponse = await client.PostAsync("/setup_robot/?robot_name=FANUC_LR_Mate_200iD", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, setupResponse.StatusCode);
|
||||
}
|
||||
|
||||
using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=true", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, activeControllerResponse.StatusCode);
|
||||
}
|
||||
|
||||
using (var connectRobotResponse = await client.PostAsync("/connect_robot/?ip=192.168.10.101", content: null))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, connectRobotResponse.StatusCode);
|
||||
}
|
||||
|
||||
using (var enableRobotResponse = await client.GetAsync("/enable_robot/"))
|
||||
{
|
||||
Assert.Equal(HttpStatusCode.OK, enableRobotResponse.StatusCode);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user