Compare commits
10 Commits
master
...
legacy-gat
| Author | SHA1 | Date | |
|---|---|---|---|
| af65ca03a0 | |||
| 91c1494cde | |||
| a6579f1e5b | |||
| c38faddbf0 | |||
| 0724efebed | |||
| 0292e077ff | |||
| 69fa3edd89 | |||
| 390d066ece | |||
| a78e6761cb | |||
| 8a20d9f507 |
17
.claude/settings.local.json
Normal file
17
.claude/settings.local.json
Normal file
@@ -0,0 +1,17 @@
|
||||
{
|
||||
"permissions": {
|
||||
"allow": [
|
||||
"Bash(git commit -m ':*)",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln -v minimal 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal 2>&1')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test FlyshotReplacement.sln --no-build -v minimal 2>&1')",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal)",
|
||||
"Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj --no-build -v minimal)",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json'\\)\\); json.load\\(open\\('.vscode/tasks.json'\\)\\); print\\('JSON valid.'\\)\")",
|
||||
"Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal')",
|
||||
"Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal')"
|
||||
]
|
||||
}
|
||||
}
|
||||
1
.gitignore
vendored
1
.gitignore
vendored
@@ -396,3 +396,4 @@ FodyWeavers.xsd
|
||||
|
||||
# JetBrains Rider
|
||||
*.sln.iml
|
||||
Config/Data/*
|
||||
|
||||
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"
|
||||
}
|
||||
]
|
||||
}
|
||||
40
AGENTS.md
40
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。
|
||||
@@ -132,9 +141,24 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../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 +172,13 @@ 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` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，J519 闭环状态判断与现场联调仍需补齐。
|
||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
||||
|
||||
11
CLAUDE.md
11
CLAUDE.md
@@ -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`
|
||||
@@ -132,6 +134,7 @@ flyshot-replacement/
|
||||
- `../analysis/ICSP_algorithm_reverse_analysis.md`
|
||||
- `../analysis/CommonMsg_protocol_analysis.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/FANUC_realtime_comm_analysis.md`
|
||||
- `../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h`
|
||||
|
||||
@@ -148,3 +151,7 @@ flyshot-replacement/
|
||||
- `Flyshot.Server.Host` 已提供最小 `/healthz`。
|
||||
- 最小集成测试已通过。
|
||||
- 解决方案构建已通过。
|
||||
- `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。
|
||||
- `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
BIN
Config/Models/LR_Mate_200iD.robot
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
BIN
Config/Models/LR_Mate_200iD_7L -.glb
Normal file
Binary file not shown.
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
BIN
Config/Models/LR_Mate_200iD_7L.robot
Normal file
Binary file not shown.
BIN
Config/Models/workpiece.stl
Normal file
BIN
Config/Models/workpiece.stl
Normal file
Binary file not shown.
1
Config/RobotConfig.json
Normal file
1
Config/RobotConfig.json
Normal file
@@ -0,0 +1 @@
|
||||
{"robot": {"use_do": true, "io_addr": [7, 8], "io_keep_cycles": 2, "acc_limit": 1, "jerk_limit": 1, "adapt_icsp_try_num": 5}, "flying_shots": {"20251015": {"traj_waypoints": [[1.047438621520996, -0.0002488955215085298, -0.0014060207176953554, 0.009022523649036884, 0.010111905634403229, 0.009573347866535187], [0.7661270499229431, -0.04437164217233658, -0.13630111515522003, -0.41718506813049316, 0.010093353688716888, 0.009594489820301533], [0.7661266326904297, 0.2170650213956833, -0.13630135357379913, -0.4171852171421051, 0.010093353688716888, 0.009594779461622238], [1.0311520099639893, -0.062108494341373444, -0.1363297700881958, 0.30276036262512207, 0.15847623348236084, 0.00956842489540577], [1.4012629985809326, -0.05120057240128517, -0.13633012771606445, 0.3027600347995758, 0.15847666561603546, 0.00956842489540577], [1.0567246675491333, 0.01165649201720953, -0.01786380261182785, -0.015170873142778873, 0.02149667963385582, 0.009576244279742241]], "shot_flags": [false, true, true, true, true, true], "offset_values": [0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4], [2, 4], [3, 4], [2, 4]]}, "TEST20251214": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8067416, 0.011661344, -0.11788314, -0.01516874, 0.021492079, 0.009567846], [0.60675246, -0.03833516, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.24043223], [0.7667507, 0.20164281, -0.11788314, 0.034831185, -0.22849938, -0.14043556], [1.1667324, 0.05164983, -0.11789217, 0.23482007, 0.021492079, -0.14043556], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0], "addr": [[], [], [3, 4, 2], [3, 4, 2], [3, 4, 2], [3, 4, 2], []]}, "UTTC_MS11": {"traj_waypoints": [[1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846], [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145], [0.96600056, 0.20607172, -0.12233179, -1.2394339, 0.10493033, 1.2958988], [0.9618476, 0.15288207, -0.14867093, -0.7176314, 0.1764264, 0.73228663], [0.76189893, -0.028442925, -0.30919823, 0.10463613, 0.5615024, -0.39399016], [1.1271763, 0.074403025, -0.27347943, -0.5227772, 0.52098846, 0.79633313], [1.0555661, 0.4026262, -0.08746306, 0.6301835, 0.09644133, -0.5463328], [1.2300354, 0.28612664, -0.23486805, -0.4868128, 0.25369516, 0.55347764], [1.2144431, -0.29855102, -0.15202847, -1.0205934, 0.13317892, 1.1246506], [1.2840607, -0.11222197, -0.16805042, -2.248135, 0.2560587, 2.4434967], [1.3189346, -0.25620222, -0.12730704, -2.285038, 0.30872014, 2.4765089], [1.502615, -0.25304365, -0.23878741, -1.2194318, 0.46674785, 1.5533328], [1.07723, -0.07387611, -0.1707704, -1.8916591, 0.38677844, 2.061968], [1.3920237, 0.08098731, -0.2672306, -0.9780007, 0.4561093, 0.9102286], [1.9016331, 0.023924276, -0.58633333, -0.8441697, 0.76730615, 1.4842151], [1.9300697, -0.06738541, -0.56542397, -0.892083, 0.77194446, 1.5293273], [2.0611632, -0.30327517, -0.54225636, -1.0395275, 0.8505439, 1.6429617], [1.0921186, -0.40034482, -0.1803499, 1.3524796, 0.6210477, -1.2159473], [1.0521278, -0.40034503, -0.1803492, 1.3524843, 0.6210471, -1.2159531], [1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846]], "shot_flags": [false, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, false, true, false], "offset_values": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "addr": [[], [2, 4], [3, 4, 2], [3, 4, 2], [4, 2], [4, 2], [3, 4], [3, 4], [4, 2], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], [4, 2], [4, 2], [4, 2], [4, 2], [4, 3], []]}, "5U": {"traj_waypoints": [[-0.95982397, 0.6331447, -1.0055008, 0.79858834, 1.1564041, -0.4260437], [-0.98353565, 0.66203266, -0.9758351, 0.8320198, 1.1455917, -0.45941326]], "shot_flags": [false, false], "offset_values": [0, 0], "addr": [[], []]}}}
|
||||
@@ -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
|
||||
|
||||
41
NLog.config
Normal file
41
NLog.config
Normal file
@@ -0,0 +1,41 @@
|
||||
<?xml version="1.0" encoding="utf-8" ?>
|
||||
<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
|
||||
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
|
||||
xsi:schemaLocation="http://www.nlog-project.org/schemas/NLog.xsd NLog.xsd"
|
||||
autoReload="true"
|
||||
throwExceptions="false"
|
||||
internalLogLevel="Off" >
|
||||
|
||||
<!-- 环境变量配置：如果 ASPNETCORE_ENVIRONMENT 为空，则默认为 Production -->
|
||||
<variable name="env" value="${environment:ASPNETCORE_ENVIRONMENT:whenEmpty=Production}"/>
|
||||
|
||||
<!-- 文件目标：按日期分文件，单文件超过 4MB 自动归档，保留最近 50 个归档文件 -->
|
||||
<targets>
|
||||
<target name="logfile" xsi:type="File"
|
||||
fileName="${basedir}/logs/${shortdate}.log"
|
||||
layout="${longdate}|${level:uppercase=true}|${threadid}|${logger}|${message}${onexception:${newline} ${exception:format=tostring}}"
|
||||
archiveFileName="${basedir}/logs/${shortdate}.{#}.log"
|
||||
archiveAboveSize="4048576"
|
||||
archiveNumbering="Sequence"
|
||||
maxArchiveFiles="50"
|
||||
concurrentWrites="true"
|
||||
keepFileOpen="false"
|
||||
encoding="utf-8" />
|
||||
|
||||
<!-- 控制台目标：开发环境使用，带颜色高亮 -->
|
||||
<target name="logconsole" xsi:type="Console"
|
||||
layout="${longdate}|${level:uppercase=true}|${threadid}|${logger}|${message}${onexception:${newline} ${exception:format=tostring}}" />
|
||||
</targets>
|
||||
|
||||
<rules>
|
||||
<!-- 压制 ASP.NET Core 的常规信息日志，只保留 Error 及以上级别。 -->
|
||||
<logger name="Microsoft.AspNetCore.*" maxlevel="Warn" final="true" />
|
||||
<logger name="Microsoft.AspNetCore.*" minlevel="Error" writeTo="logconsole,logfile" />
|
||||
|
||||
<!-- 开发环境：显示控制台 + 详细文件，最低 Debug -->
|
||||
<logger name="*" minlevel="Debug" writeTo="logconsole,logfile" condition="equals('${var:env}','Development')" />
|
||||
|
||||
<!-- 生产环境：仅文件，最低 Info -->
|
||||
<logger name="*" minlevel="Info" writeTo="logfile" condition="not_equals('${var:env}','Development')" />
|
||||
</rules>
|
||||
</nlog>
|
||||
106
README.md
106
README.md
@@ -4,7 +4,7 @@
|
||||
|
||||
当前目标：
|
||||
|
||||
- 兼容现有 `50001/TCP+JSON` 上层接入语义
|
||||
- 以新的 ASP.NET Core HTTP API 作为唯一上层接口
|
||||
- 重写轨迹生成、触发时序和 FANUC 实时控制链路
|
||||
- 提供 Web 状态监控页面
|
||||
- 在 Windows 和 Linux 上运行完整后台服务
|
||||
@@ -13,12 +13,112 @@
|
||||
|
||||
- 这是长期运行的无头后台服务，不是 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` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。
|
||||
- 2026-04-28 UTTC 抓包确认：UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`，`JointDetialTraj.txt` 为弧度制 `rad`，`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹。
|
||||
- 2026-04-30 本机 `50001/TCP+JSON` 抓包确认：`ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求只显式携带规划方法、保存、缓存和等待参数，不携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此旧系统不可见的有效规划限制不再继续假设来自公开链路，新系统按 replacement-only 内部参数限制规划加速度。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送能力；稠密轨迹下发已按 `speed_ratio` 做执行时间缩放，并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。
|
||||
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹：当前关节到目标关节的关节空间直线，五次 smoothstep 起停，按 `status=15` 运动窗口复现 `40/55/77` 点，并由 J519 层完成 `rad -> deg` 下发。
|
||||
- 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`，不再创建独立轨迹存储文件。
|
||||
|
||||
当前 Todo：
|
||||
单位约定总览：
|
||||
|
||||
- 规划层、`JointDetialTraj.txt` 和运行时内部关节轨迹，默认按弧度制 `rad` 理解。
|
||||
- `UDP 60015` J519 命令 `target[0..5]` 和响应关节反馈按角度制 `deg` 理解；运行时下发前必须显式执行 `rad -> deg` 转换。
|
||||
- `TCP 10010` 状态通道是混合单位：`pose[0..2]` 更像 `mm`，`pose[3..5]` 更像 `deg`，`joint_or_ext[0..5]` 当前现场抓包更支持按 `rad` 理解。
|
||||
- 不要把“关节角”默认当成统一单位；在规划、状态监控和 J519 执行三条链路之间必须明确标注 `rad/deg`。
|
||||
|
||||
当前现场主链路的单位流转可简化为：
|
||||
|
||||
| 位置 | 内容 | 当前更可信单位 |
|
||||
| --- | --- | --- |
|
||||
| 规划输入 / 轨迹算法 | 关节角 | `rad` |
|
||||
| `JointDetialTraj.txt` / `JointTraj.txt` | 关节角 | `rad` |
|
||||
| 运行时下发前内部轨迹 | 关节角 | `rad` |
|
||||
| `UDP 60015` 命令 `target[0..5]` | 关节目标 | `deg` |
|
||||
| `UDP 60015` 响应 `Joint` | 关节反馈 | `deg` |
|
||||
| `TCP 10010` `pose[0..2]` | `X/Y/Z` | `mm` |
|
||||
| `TCP 10010` `pose[3..5]` | 姿态角 | `deg` |
|
||||
| `TCP 10010` `joint_or_ext[0..5]` | 关节状态 | 更像 `rad` |
|
||||
| `TCP 10010` `joint_or_ext[6..8]` | 扩展轴槽位 | 当前样本为 `0` |
|
||||
|
||||
`TCP 10010` 的正式字段表、样例帧和已确认/待确认说明见 `docs/fanuc-10010-state-frame.md`。
|
||||
|
||||
开发约定：
|
||||
|
||||
- 建议从 `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` 网关
|
||||
- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.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` 全部贯通到规划和执行链路。
|
||||
- [x] 将上传飞拍轨迹统一保存到运行目录 `Config/RobotConfig.json` 的 `flying_shots` 节点。
|
||||
- [x] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流：见 `docs/fanuc-field-runtime-workflow.md`。
|
||||
|
||||
2. 轨迹规划
|
||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||
- [ ] 新增 replacement-only 的 `planning_acceleration_scale` 规划加速度校准参数，用于复现旧服务端公开链路中抓不到的保守 effective limits；该参数只影响规划结果，不影响运行时 `speed_ratio`。
|
||||
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
||||
- [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本：输入为 `rad`，下发为 `deg`，并按 `speed_ratio` 拉伸时间轴；覆盖 `2026042802-0.5/0.7/1.pcap`。
|
||||
- [x] 补齐飞拍 `save_traj` / `SaveTrajInfo` 的规划结果导出，将关节关键点、稠密关节轨迹、笛卡尔关键点、稠密笛卡尔轨迹和 ShotEvents 写入 `Config/Data/<name>`。
|
||||
|
||||
3. FANUC TCP 10012 命令通道
|
||||
- [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。
|
||||
- [x] 所有命令响应必须检查 `result_code`，失败时返回可诊断错误，而不是只更新本地缓存。
|
||||
|
||||
4. FANUC TCP 10010 状态通道
|
||||
- [x] 用 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||
- [x] 补充 `TCP 10010` 正式字段表与已确认/待确认说明：见 `docs/fanuc-10010-state-frame.md`。
|
||||
- [x] 补充断线清理和异常帧拒绝测试。
|
||||
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
||||
|
||||
5. FANUC UDP 60015 J519 运动链路
|
||||
- [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。
|
||||
- [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整 8ms 发送索引/时间尺度：第 `k` 个 J519 目标采样 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- [x] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查；若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪，则拒绝稠密轨迹执行。
|
||||
- [x] 校验序号递增、状态包 sequence 校准、响应滞后、丢包、停止包和最后一帧语义：UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`；J519 客户端测试覆盖收到 status 后按 status sequence 回发命令和 type 2 状态输出停止包。
|
||||
- [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。
|
||||
- [x] 将 `MoveJoint` 从单点最终目标改为临时 PTP 稠密轨迹：按 `status=15` 运动窗口统计，speed=1 抓包 40 点，speed=0.7 抓包 55 点，speed=0.5 抓包 77 点，路径为关节空间直线 + smoothstep 进度。
|
||||
- [x] `ExecuteFlyShotTraj(move_to_start=true)` 复用临时 PTP 稠密轨迹移动到规划起点，并等待运行时完成后再启动飞拍轨迹，避免第一帧 J519 目标突变导致控制柜报警。
|
||||
- [x] `ExecuteFlyShotTraj(wait=true)` 等待正式飞拍轨迹执行完成后再返回；HTTP `/execute_flyshot/` 已接入旧抓包中的 `wait` 字段，默认值为 `true`。
|
||||
|
||||
6. 真机联调与运行安全
|
||||
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。
|
||||
- [x] 实体机复核运行速度对轨迹生成时间的影响：`speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹；运行阶段仅 J519 下发时长和包数按 `speed_ratio` 拉伸，UTTC_MS11 参考值为约 `7.4s` 与 `10.56s`。
|
||||
- [ ] 增加急停、伺服未就绪、程序未启动、网络断开、控制柜拒收命令等故障路径处理。
|
||||
- [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。
|
||||
- [ ] 增加运行日志、协议摘要日志和状态快照导出，便于现场排查。
|
||||
|
||||
7. 发布与部署
|
||||
- [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。
|
||||
- [ ] 补充生产配置模板、端口说明和现场部署检查表。
|
||||
- [ ] 给 Web 状态页增加程序状态和最近报警显示；J519 状态位已通过快照和状态页显示。
|
||||
|
||||
143
docs/controller-client-api-compatibility-requirements.md
Normal file
143
docs/controller-client-api-compatibility-requirements.md
Normal file
@@ -0,0 +1,143 @@
|
||||
# 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 当前策略”
|
||||
- 真机未接通前，允许实现层返回稳定错误或模拟状态，但不能反过来污染逆向文档
|
||||
- `50001/TCP+JSON` 抓包已经覆盖 `SetSpeedRatio` 与 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`，请求中没有显式 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk` 字段；因此规划限制的补齐必须作为 replacement-only 策略记录，不能写成旧公开 API 合同。
|
||||
|
||||
## 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, wait)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。
|
||||
- 规划阶段会继续消费旧配置中的 `acc_limit / jerk_limit`。如果现场需要复现旧服务端不可见的保守约束，replacement 设计上使用内部 `planning_acceleration_scale` 限制规划加速度；该字段不属于旧 `ControllerClient` 公开 API，也不会通过 `50001` JSON 下发。
|
||||
- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。
|
||||
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。
|
||||
- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是按 `floor(duration / (0.008 * speed_ratio)) + 1` 形成 J519 运行包。
|
||||
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。
|
||||
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。
|
||||
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||
562
docs/controller-client-api-reverse-engineering.md
Normal file
562
docs/controller-client-api-reverse-engineering.md
Normal file
@@ -0,0 +1,562 @@
|
||||
# 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`
|
||||
- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。实发规则为 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。
|
||||
- 飞拍轨迹相关额外字符串线索：
|
||||
- `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. 最后实现飞拍轨迹相关接口，并把本文档中的“待确认问题”逐项收敛成兼容测试。
|
||||
165
docs/fanuc-10010-state-frame.md
Normal file
165
docs/fanuc-10010-state-frame.md
Normal file
@@ -0,0 +1,165 @@
|
||||
# FANUC TCP 10010 状态帧字段说明
|
||||
|
||||
本文档整理当前现场真实抓包中 `TCP 10010` 状态通道的已确认字段布局，并明确哪些结论已经由抓包和代码验证，哪些仍然只是工作假设。
|
||||
|
||||
读取时间：2026-05-03
|
||||
|
||||
## 1. 结论范围
|
||||
|
||||
本文基于以下证据整理：
|
||||
|
||||
- `../analysis/UTTC_20260428_packet_validation.md`
|
||||
- `../analysis/J519_stream_motion_analysis.md`
|
||||
- `Rvbust/uttc-20260428/20260428.pcap`
|
||||
- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs`
|
||||
- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs`
|
||||
|
||||
当前结论仅覆盖现场已确认的 `R30iB + RVBUSTSM` 这一路状态通道行为，不提前推广为所有 FANUC 机型或所有旧版本协议的通用结论。
|
||||
|
||||
## 2. 通道性质
|
||||
|
||||
真实抓包显示，`TCP 10010` 是控制柜到上位机的单向状态流：
|
||||
|
||||
- 上位机先主动建立 TCP 连接。
|
||||
- 建连后，带应用层 payload 的业务包全部来自 `192.168.10.11:10010 -> 192.168.10.10:41726`。
|
||||
- 上位机在该通道上只回 TCP `ACK`，没有观察到应用层请求体。
|
||||
|
||||
因此当前实现应把 `10010` 当作“持续推送的固定长度状态帧”处理，而不是像 `TCP 10012` 那样按请求/响应语义建模。
|
||||
|
||||
## 3. 整体布局
|
||||
|
||||
当前现场抓包确认，状态帧固定为 `90B`：
|
||||
|
||||
```text
|
||||
doz 3 bytes
|
||||
length u32 = 90
|
||||
msg_id u32
|
||||
pose[6] f32
|
||||
joint_or_ext[9] f32
|
||||
tail[4] u32
|
||||
zod 3 bytes
|
||||
```
|
||||
|
||||
其中：
|
||||
|
||||
- 帧头 magic 固定为 `doz`
|
||||
- 帧尾 magic 固定为 `zod`
|
||||
- 长度字段固定为 `0x5a`，即 `90`
|
||||
- 当前全量抓包中 `msg_id` 恒为 `0`
|
||||
- `tail[4]` 当前全量抓包中恒为 `(2, 0, 0, 1)`
|
||||
|
||||
## 4. 样例帧
|
||||
|
||||
以下样例帧来自 `20260428.pcap` 中首条 `tcp.port == 10010 && tcp.len > 0` 的 payload：
|
||||
|
||||
```text
|
||||
646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64
|
||||
```
|
||||
|
||||
对应解析值：
|
||||
|
||||
- `pose[6]`
|
||||
- `273.26715`
|
||||
- `483.85544`
|
||||
- `467.73764`
|
||||
- `-57.253044`
|
||||
- `-89.618675`
|
||||
- `-62.21883`
|
||||
- `joint_or_ext[9]`
|
||||
- `1.0567309`
|
||||
- `0.011662138`
|
||||
- `-0.01789339`
|
||||
- `-0.015160045`
|
||||
- `0.02149596`
|
||||
- `0.009560025`
|
||||
- `0`
|
||||
- `0`
|
||||
- `0`
|
||||
- `tail[4]`
|
||||
- `2`
|
||||
- `0`
|
||||
- `0`
|
||||
- `1`
|
||||
|
||||
## 5. 正式字段表
|
||||
|
||||
| 偏移 | 长度 | 类型 | 样例值(hex) | 样例值(解析后) | 当前推断含义 |
|
||||
| --- | --- | --- | --- | --- | --- |
|
||||
| `0x00` | `3` | `char[3]` | `64 6f 7a` | `"doz"` | 固定帧头 magic |
|
||||
| `0x03` | `4` | `u32 be` | `00 00 00 5a` | `90` | 帧总长度 |
|
||||
| `0x07` | `4` | `u32 be` | `00 00 00 00` | `0` | `msg_id`，当前抓包全为 `0` |
|
||||
| `0x0B` | `4` | `f32 be` | `43 88 a2 32` | `273.26715` | `pose[0]`，推断为 TCP `X(mm)` |
|
||||
| `0x0F` | `4` | `f32 be` | `43 f1 ed 7f` | `483.85544` | `pose[1]`，推断为 TCP `Y(mm)` |
|
||||
| `0x13` | `4` | `f32 be` | `43 e9 de 6b` | `467.73764` | `pose[2]`，推断为 TCP `Z(mm)` |
|
||||
| `0x17` | `4` | `f32 be` | `c2 65 03 1e` | `-57.253044` | `pose[3]`，推断为姿态角 `W(deg)` |
|
||||
| `0x1B` | `4` | `f32 be` | `c2 b3 3c c3` | `-89.618675` | `pose[4]`，推断为姿态角 `P(deg)` |
|
||||
| `0x1F` | `4` | `f32 be` | `c2 78 e0 15` | `-62.21883` | `pose[5]`，推断为姿态角 `R(deg)` |
|
||||
| `0x23` | `4` | `f32 be` | `3f 87 42 f5` | `1.0567309` | `joint_or_ext[0]`，推断为 `J1(rad)` |
|
||||
| `0x27` | `4` | `f32 be` | `3c 3f 12 8d` | `0.011662138` | `joint_or_ext[1]`，推断为 `J2(rad)` |
|
||||
| `0x2B` | `4` | `f32 be` | `bc 92 95 29` | `-0.01789339` | `joint_or_ext[2]`，推断为 `J3(rad)` |
|
||||
| `0x2F` | `4` | `f32 be` | `bc 78 61 d6` | `-0.015160045` | `joint_or_ext[3]`，推断为 `J4(rad)` |
|
||||
| `0x33` | `4` | `f32 be` | `3c b0 18 4c` | `0.02149596` | `joint_or_ext[4]`，推断为 `J5(rad)` |
|
||||
| `0x37` | `4` | `f32 be` | `3c 1c a1 a7` | `0.009560025` | `joint_or_ext[5]`，推断为 `J6(rad)` |
|
||||
| `0x3B` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[6]`，扩展轴槽位，当前样本恒 `0` |
|
||||
| `0x3F` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[7]`，扩展轴槽位，当前样本恒 `0` |
|
||||
| `0x43` | `4` | `f32 be` | `00 00 00 00` | `0` | `joint_or_ext[8]`，扩展轴槽位，当前样本恒 `0` |
|
||||
| `0x47` | `4` | `u32 be` | `00 00 00 02` | `2` | `tail[0]`，诊断状态字，物理语义未坐实 |
|
||||
| `0x4B` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[1]`，诊断状态字，物理语义未坐实 |
|
||||
| `0x4F` | `4` | `u32 be` | `00 00 00 00` | `0` | `tail[2]`，诊断状态字，物理语义未坐实 |
|
||||
| `0x53` | `4` | `u32 be` | `00 00 00 01` | `1` | `tail[3]`，诊断状态字，物理语义未坐实 |
|
||||
| `0x57` | `3` | `char[3]` | `7a 6f 64` | `"zod"` | 固定帧尾 magic |
|
||||
|
||||
## 6. 已确认结论
|
||||
|
||||
### 6.1 已由真实抓包确认
|
||||
|
||||
1. `TCP 10010` 是独立状态流，不是 `TCP 10012` 的请求/响应复用。
|
||||
2. 当前现场状态帧固定为 `90B`，不是早期静态分析里出现过的 `134B`。
|
||||
3. `msg_id` 在 `20260428.pcap` 当前全量样本中恒为 `0`。
|
||||
4. `tail[4]` 在 `20260428.pcap` 当前全量样本中恒为 `(2, 0, 0, 1)`。
|
||||
5. `pose[6]` 的量纲表现符合 `X/Y/Z(mm) + W/P/R(deg)`。
|
||||
6. `joint_or_ext[6..8]` 在当前现场样本中恒为 `0`。
|
||||
|
||||
### 6.2 已由数值范围和交叉对照强支持
|
||||
|
||||
1. `joint_or_ext[0..5]` 更像关节角 `rad`，而不是 `deg`。
|
||||
2. 该判断与 `../analysis/UTTC_20260428_packet_validation.md` 的结论一致。
|
||||
3. 该判断也与 `UDP 60015` 响应包中的关节 `deg` 形成互补关系：二者不能简单视作同单位直接复用。
|
||||
|
||||
## 7. 待确认项
|
||||
|
||||
以下内容当前不要写死为最终协议真义：
|
||||
|
||||
1. `tail[4]` 四个 `u32` 分别代表什么控制器语义。
|
||||
2. `msg_id` 是否在其他控制柜版本、程序状态或异常态下会出现非零值。
|
||||
3. `pose[3..5]` 是否可以严格命名为 FANUC 标准 `W/P/R`，还是只是与其数值表现一致。
|
||||
4. `joint_or_ext[6..8]` 在带外部轴的现场是否仍复用同一布局。
|
||||
|
||||
## 8. 与当前代码实现的对齐情况
|
||||
|
||||
当前仓库里 `Flyshot.Runtime.Fanuc` 已按 `90B` 固定帧解析：
|
||||
|
||||
- `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs`
|
||||
- `tests/Flyshot.Core.Tests/FanucProtocolTests.cs`
|
||||
|
||||
当前实现已经与抓包对齐的部分：
|
||||
|
||||
1. 固定长度 `90B`
|
||||
2. `doz ... zod` 帧头帧尾校验
|
||||
3. `pose[6] + joint_or_ext[9] + tail[4]` 的字节布局
|
||||
4. `tail[4]` 原样保留到 `ControllerStateSnapshot.StateTailWords`
|
||||
|
||||
当前仍建议后续关注的点：
|
||||
|
||||
1. `FanucStateFrame` 已把该字段从 `JointDegrees` 更正为 `JointRadians`，后续新增代码应继续沿用弧度制命名。
|
||||
2. 如果后续状态页或运行时逻辑需要直接展示该通道关节值，仍需明确标注这是 `10010` 的弧度值，避免和 `UDP 60015` 的 degree 语义混淆。
|
||||
|
||||
## 9. 建议用法
|
||||
|
||||
在当前 replacement 实现里，`TCP 10010` 更适合作为以下用途：
|
||||
|
||||
1. 提供机器人当前笛卡尔位姿和关节反馈快照。
|
||||
2. 提供状态通道是否健康、是否陈旧的连接诊断依据。
|
||||
3. 保留 `tail[4]` 原始状态字，供现场排错或后续继续逆向。
|
||||
|
||||
当前不建议直接用 `tail[4]` 去驱动明确业务判断，除非后续拿到新的现场对照证据。
|
||||
181
docs/fanuc-field-runtime-workflow.md
Normal file
181
docs/fanuc-field-runtime-workflow.md
Normal file
@@ -0,0 +1,181 @@
|
||||
# FANUC Field Runtime Workflow
|
||||
|
||||
本文档记录当前现场主链路的 HTTP 调用顺序，以及每一步在 FANUC 三条真机通道上的动作。它替代旧 `ControllerClient` 工作流说明；旧 `50001/TCP+JSON` 入口不再作为运行目标。
|
||||
|
||||
## 1. 初始化
|
||||
|
||||
推荐使用聚合端点完成当前现场的一次性初始化：
|
||||
|
||||
```bash
|
||||
POST /init_mpc_robt
|
||||
{
|
||||
"server_ip": "127.0.0.1",
|
||||
"port": 50001,
|
||||
"robot_name": "FANUC_LR_Mate_200iD",
|
||||
"robot_ip": "192.168.10.11",
|
||||
"sim": false
|
||||
}
|
||||
```
|
||||
|
||||
该端点内部顺序：
|
||||
|
||||
1. `ConnectServer(server_ip, port)`：兼容旧参数形状，仅记录服务连接语义。
|
||||
2. `SetUpRobot(robot_name)`：加载机器人配置、关节限制和伺服周期。
|
||||
3. `SetActiveController(sim)`：选择仿真或 FANUC 真机运行时。
|
||||
4. `Connect(robot_ip)`：真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。
|
||||
5. `EnableRobot(2)`：真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`，随后允许 J519 在收到机器人 UDP status 包后回发下一帧命令。
|
||||
|
||||
也可以使用拆分端点按同样顺序调用：
|
||||
|
||||
```text
|
||||
POST /connect_server/?server_ip=127.0.0.1&port=50001
|
||||
POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD
|
||||
POST /set_active_controller/?sim=false
|
||||
POST /connect_robot/?ip=192.168.10.11
|
||||
GET /enable_robot/?buffer_size=2
|
||||
```
|
||||
|
||||
## 2. 参数设置
|
||||
|
||||
规划约束参数：
|
||||
|
||||
当前现场抓包已经确认，`50001/TCP+JSON` 的 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 请求不会显式携带 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此新系统把规划约束分成两层处理：
|
||||
|
||||
1. 旧 `RobotConfig.json` 中已有的 `acc_limit / jerk_limit` 继续作为模型加载时的基础倍率。
|
||||
2. 若旧系统导出的 `JointTraj.txt` 明显比当前 C# 规划更慢，使用 replacement-only 的内部校准参数限制规划阶段加速度，设计字段为 `planning_acceleration_scale`。
|
||||
|
||||
`planning_acceleration_scale` 只影响 `JointTraj.txt` 这类规划结果时间轴，不下发到 FANUC 控制柜，也不改变 J519 发送周期。若需要临时整体验证，也可以使用当前已有的 `planning_speed_scale`，但它是新系统兼容开关，不是旧抓包中出现的字段。
|
||||
|
||||
速度倍率：
|
||||
|
||||
```bash
|
||||
POST /set_speedRatio/
|
||||
{ "speed": 0.7 }
|
||||
```
|
||||
|
||||
真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴：
|
||||
|
||||
```text
|
||||
t_traj = k * 0.008 * speed_ratio
|
||||
send_count = floor(duration / (0.008 * speed_ratio)) + 1
|
||||
```
|
||||
|
||||
TCP 和普通 IO：
|
||||
|
||||
```text
|
||||
POST /set_tcp/ body: { "x": 0, "y": 0, "z": 0 }
|
||||
GET /get_tcp/
|
||||
POST /set_io/?port=7&value=true&io_type=DO
|
||||
GET /get_io/?port=7&io_type=DO
|
||||
```
|
||||
|
||||
飞拍触发 IO 不走独立 `TCP 10012 SetIO`，而是嵌入 `UDP 60015` J519 命令包的 `write_io_type/index/mask/value` 字段。
|
||||
|
||||
## 3. 点到点 MoveJoint
|
||||
|
||||
```bash
|
||||
POST /move_joint/
|
||||
{ "joints": [0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145] }
|
||||
```
|
||||
|
||||
`MoveJoint` 不再直接把最终关节写成单个 J519 目标，而是按现场抓包确认的 PTP 临时轨迹执行：
|
||||
|
||||
1. 从当前运行时状态读取当前关节坐标，单位为 `rad`。
|
||||
2. 以当前关节和目标关节构造关节空间直线。
|
||||
3. 用五次 smoothstep `10u^3 - 15u^4 + 6u^5` 生成起停平滑的进度。
|
||||
4. 真机执行时仍由 J519 层把 `rad` 转成 `deg`，并按当前 `speed_ratio` 重采样。
|
||||
|
||||
已确认抓包按响应 `status=15` 运动窗口统计：
|
||||
|
||||
| 抓包 | speed_ratio | 运动窗口点数 | 运动窗口时长 |
|
||||
|------|-------------|----------------------|----------|
|
||||
| `2026042802-mvpoint.pcap` | 1.0 | 40 | 约 0.312s |
|
||||
| `2026042802-mvpoint0.7.pcap` | 0.7 | 55 | 约 0.432s |
|
||||
| `2026042802-mvpoint0.5.pcap` | 0.5 | 77 | 约 0.608s |
|
||||
|
||||
抓包命令流在运动窗口前后还会持续发送保持不变的起点/终点目标；功能复刻以 `status=15` 运动窗口为点数口径，并把最后一个采样点压到目标关节。实际目标几乎严格位于“起点 -> 终点”的同一条关节空间直线上，`speed_ratio` 体现为 J519 发送时间轴上的减速重采样，而不是改变路径形状。
|
||||
|
||||
## 4. 飞拍轨迹
|
||||
|
||||
上传：
|
||||
|
||||
```bash
|
||||
POST /upload_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"waypoints": [[...]],
|
||||
"shot_flags": [false, true],
|
||||
"offset_values": [0, 0],
|
||||
"addrs": [[1, 3]]
|
||||
}
|
||||
```
|
||||
|
||||
校验：
|
||||
|
||||
```bash
|
||||
POST /is_flyShotTrajValid/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false
|
||||
}
|
||||
```
|
||||
|
||||
执行：
|
||||
|
||||
```bash
|
||||
POST /execute_flyshot/
|
||||
{
|
||||
"name": "UTTC_MS11",
|
||||
"move_to_start": true,
|
||||
"method": "self-adapt-icsp",
|
||||
"save_traj": false,
|
||||
"use_cache": true,
|
||||
"wait": true
|
||||
}
|
||||
```
|
||||
|
||||
执行链路：
|
||||
|
||||
1. 从上传缓存读取 waypoint、shot flag、offset、IO 地址组。
|
||||
2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹；规划阶段先应用 `acc_limit / jerk_limit`，再应用 replacement-only 的规划加速度校准参数。
|
||||
3. 生成 `TrajectoryDoEvent`，把拍照触发绑定到轨迹时间。
|
||||
4. 若 `move_to_start=true`，先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点，并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹，避免第一帧 J519 目标从当前位置跳到起点。
|
||||
5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`，命令实际发包由机器人 UDP status 包驱动。
|
||||
6. 若 `wait=true`，正式飞拍轨迹启动后继续等待运行时 `IsInMotion=false`，机器人执行完整条飞拍轨迹后 HTTP 才返回；`wait=false` 时启动后立即返回。
|
||||
7. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪，则拒绝执行。
|
||||
8. 周期命令中嵌入 IO 脉冲；当前 UTTC 抓包确认 mask 集合为 `10/12/14`，共 17 个 set 脉冲和 17 个 clear 帧。
|
||||
|
||||
`method="doubles"` 当前明确返回未实现；现场主链路使用 `icsp` / `self-adapt-icsp`。
|
||||
|
||||
## 5. 停止与断开
|
||||
|
||||
```text
|
||||
GET /stop_move/
|
||||
GET /disable_robot/
|
||||
POST /disconnect_robot/
|
||||
```
|
||||
|
||||
真机模式下：
|
||||
|
||||
- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。
|
||||
- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包，然后 `StopProg("RVBUSTSM")`。
|
||||
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
|
||||
|
||||
## 6. 现场抓包覆盖
|
||||
|
||||
`tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs` 直接解析以下抓包并与 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 对比：
|
||||
|
||||
| 抓包 | 速度 | 运行 J519 点数 | 发送时长 |
|
||||
|------|------|----------------|----------|
|
||||
| `2026042802-0.5.pcap` | 0.5 | 1851 | 14.800309s |
|
||||
| `2026042802-0.7.pcap` | 0.7 | 1322 | 10.568313s |
|
||||
| `2026042802-1.pcap` | 1.0 | 926 | 7.400125s |
|
||||
|
||||
测试同时检查：
|
||||
|
||||
- 主运行窗口命令序号连续，无重复 seq；J519 客户端单元测试覆盖按最新 status sequence 回发命令。
|
||||
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
|
||||
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
|
||||
- `lastData=0`，结束运动当前依赖 J519 packet type 2 状态输出停止包；`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本，停止语义后续单独验证。
|
||||
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
|
||||
187
docs/fanuc-socket-implementation-plan.md
Normal file
187
docs/fanuc-socket-implementation-plan.md
Normal file
@@ -0,0 +1,187 @@
|
||||
# FANUC 真机协议 Socket 通信层实现计划
|
||||
|
||||
## 上下文
|
||||
|
||||
状态更新：本计划中的 Socket 客户端和 `FanucControllerRuntime` 改造已经落地；当前事实以 `README.md` 和 `docs/fanuc-field-runtime-workflow.md` 为准。本文保留为实现过程记录。
|
||||
|
||||
计划制定时 `flyshot-replacement` 项目已完成：
|
||||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
2026-04-28 `Rvbust/uttc-20260428/20260428.pcap` 新增约束：
|
||||
|
||||
- `TCP 10010` 状态帧继续确认为固定 `90B`。
|
||||
- `UDP 60015` 命令 `target[0..5]` 为关节角 `deg`，而 `JointDetialTraj.txt` 为 `rad`。
|
||||
- `speed_ratio=0.7` 在本抓包中表现为 UDP 下发时间轴约 `1.427730x` 拉伸；机器人侧 `TCP 10012` 未抓到 `0x2207 SetSpeedRatio`。
|
||||
- `UTTC_MS11` 的 17 个飞拍触发点与 17 个 UDP IO 脉冲一一对齐，`io_keep_cycles=2` 对应约两周期清零。
|
||||
|
||||
**历史缺失项（已完成）**：计划制定时 `FanucControllerRuntime` 仍是纯内存状态桩。当前实现已经改为持有 `FanucCommandClient`、`FanucStateClient` 和 `FanucJ519Client`，真机模式会建立三条通道并从状态/J519 响应读取运行状态。
|
||||
|
||||
## 目标
|
||||
|
||||
将 `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` 字段布局实现；注意 2026-04-28 真实运行抓包未出现机器人侧 `0x2207`，执行链路仍必须在 UDP 发送时间尺度上应用当前速度倍率
|
||||
- `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]`。
|
||||
- `Rvbust/uttc-20260428/20260428.pcap` 再次确认 `10010` 状态帧固定 90B，平均间隔约 25.6ms。
|
||||
- 尾部状态字当前只作为 `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()` — 清理
|
||||
|
||||
执行注意事项：
|
||||
|
||||
- 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
|
||||
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。
|
||||
- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
|
||||
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
||||
|
||||
**测试**：`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)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 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 发送路点，并按当前 `speed_ratio` 推进原始轨迹时间
|
||||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|
||||
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
232
docs/fanuc-stream-motion-manual-notes.md
Normal file
@@ -0,0 +1,232 @@
|
||||
# FANUC Stream Motion 文档要点与实现差异
|
||||
|
||||
本文记录 `../FANUC_stream_motion.pdf` 中与本仓库 `Flyshot.Runtime.Fanuc` 直接相关的重点，并对照当前实现状态。
|
||||
|
||||
读取时间：2026-05-03
|
||||
|
||||
## 1. 文档定位
|
||||
|
||||
`FANUC_stream_motion.pdf` 对应 FANUC `Stream motion` 功能，选项号为 `A05B-2600-J519`。它描述的是外部设备通过以太网实时发送期望位置，让机器人按外部生成路径运动的控制方式。
|
||||
|
||||
文档明确要求外部设备自行生成满足机器人约束的路径，包括速度、加速度、jerk、可达性、姿态连续性等。FANUC 不提供完整运动学、逆解和碰撞检测公式。
|
||||
|
||||
## 2. 使用前提与示教程序
|
||||
|
||||
1. 机器人侧需要安装 J519 stream motion 选项。
|
||||
2. 物理网口通过 `$STMO.$PHYS_PORT` 选择，`1` 表示 `CD38A`，`2` 表示 `CD38B`。
|
||||
3. 机器人程序必须包含成对的 `IBGN start[*]` 和 `IBGN end[*]` 指令，二者编号必须一致，`start` 必须在 `end` 前一行。
|
||||
4. `IBGN start[*]` 执行期间，机器人根据外部设备发来的期望位置运动；`IBGN end[*]` 之后程序继续执行。
|
||||
5. 执行时要求 `AUTO` 模式和 `100% OVERRIDE`。
|
||||
|
||||
当前实现中的 `FanucControllerRuntime.EnableRobot()` 会按现场抓包流程启动 `RVBUSTSM` 程序，并随后允许 J519 在收到机器人 UDP status 包后回发命令。是否满足 `AUTO / 100% OVERRIDE / IBGN start` 已到位，当前只通过 J519 状态位和现场程序行为间接判断，没有在代码里读取或设置这些控制器状态。
|
||||
|
||||
## 3. UDP 60015 协议结构
|
||||
|
||||
协议使用 UDP，大端字节序，机器人侧端口为 `60015`。通信周期通常为 `8ms`，部分机型支持 `4ms`。状态包输出可以在任意时间通过 start/stop 控制包启停，不要求已经进入 `IBGN start[*]`。
|
||||
|
||||
### 3.1 状态输出 start 包
|
||||
|
||||
外部设备发给机器人：
|
||||
|
||||
| 字段 | 长度 | 值 |
|
||||
| --- | --- | --- |
|
||||
| Packet type | 4B | `0` |
|
||||
| Version | 4B | `1` |
|
||||
|
||||
当前实现：`FanucJ519Protocol.PackInitPacket()` 已按 8B 大端控制包实现，`FanucJ519Client.ConnectAsync()` 连接后立即发送。
|
||||
|
||||
### 3.2 状态包
|
||||
|
||||
机器人发给外部设备，长度为 `132B`：
|
||||
|
||||
| 偏移 | 字段 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `0x00` | Packet type | `0` |
|
||||
| `0x04` | Version | `1` |
|
||||
| `0x08` | Sequence No. | 状态包序号，发送 start 包后从 `1` 重新开始 |
|
||||
| `0x0c` | Status | bit0 接受命令、bit1 已收到命令、bit2 SYSRDY、bit3 运动中 |
|
||||
| `0x0d..0x12` | Read I/O 回显和值 | 回显命令包中的读取 IO 类型、索引、掩码，并返回 16 点 IO 值 |
|
||||
| `0x14` | Timestamp | ms 单位，2ms 分辨率 |
|
||||
| `0x18..0x38` | Cartesian / external axis | `X/Y/Z/W/P/R` 加 3 个扩展轴 |
|
||||
| `0x3c..0x5c` | Joint | `J1..J9`，单位 degree |
|
||||
| `0x60..0x80` | Motor current | `J1..J9` 电流，单位 A |
|
||||
|
||||
当前实现：`FanucJ519Protocol.ParseResponse()` 已解析 `132B` 状态包，并暴露 `AcceptsCommand`、`ReceivedCommand`、`SystemReady`、`RobotInMotion` 四个状态位。`FanucControllerRuntime.GetSnapshot()` 也会把最新 J519 状态写进快照。
|
||||
|
||||
### 3.3 命令包
|
||||
|
||||
外部设备发给机器人，长度为 `64B`：
|
||||
|
||||
| 偏移 | 字段 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `0x00` | Packet type | `1` |
|
||||
| `0x04` | Version | `1` |
|
||||
| `0x08` | Sequence No. | 第一包应等于刚收到的状态包序号，后续逐包递增 |
|
||||
| `0x0c` | Last data | 正常为 `0`；结束外部控制时最后一包设为 `1` |
|
||||
| `0x0d..0x11` | Reading I/O | 可读取最多 16 个连续 IO 点 |
|
||||
| `0x12` | Data format | `0` 笛卡尔，`1` 关节 |
|
||||
| `0x13..0x19` | Writing I/O | 可写入最多 16 个连续 IO 点 |
|
||||
| `0x1a` | unused | 2B |
|
||||
| `0x1c..0x3c` | target[9] | 9 个 f32 目标值；关节格式时单位 degree |
|
||||
|
||||
当前实现：`FanucJ519Protocol.PackCommandPacket()` 已按上述布局打包，默认 `dataStyle=1`，也就是关节格式。运行时会把规划输出的弧度制关节轨迹转换为 degree 后下发。
|
||||
|
||||
### 3.4 状态输出 stop 包
|
||||
|
||||
外部设备发给机器人：
|
||||
|
||||
| 字段 | 长度 | 值 |
|
||||
| --- | --- | --- |
|
||||
| Packet type | 4B | `2` |
|
||||
| Version | 4B | `1` |
|
||||
|
||||
文档把它定义为“停止状态包输出”的控制包，不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。
|
||||
|
||||
当前实现：`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`，而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一；已有多数 UTTC 抓包显示主运行窗口 `LastData=0`，但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本，后续应单独校准停止语义。
|
||||
|
||||
## 4. 通信时序重点
|
||||
|
||||
文档推荐的时序是：
|
||||
|
||||
1. 外部设备发送状态输出 start 包。
|
||||
2. 机器人每个通信周期输出状态包。
|
||||
3. 机器人程序执行到 `IBGN start[*]` 后，状态包 bit0 变为 `1`，表示等待命令包。
|
||||
4. 外部设备收到 bit0 为 `1` 的状态包后，立即发送第一帧命令包，第一帧命令序号应等于刚收到的状态包序号。
|
||||
5. 后续每收到一个状态包，外部设备应立即发送下一帧命令包。
|
||||
6. 结束命令通信时，发送 `Last data=1` 的最后一帧命令包。
|
||||
|
||||
当前实现对照：
|
||||
|
||||
1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令，不再由上位机本地固定 8ms 发送循环主动发包。
|
||||
2. 命令包 sequence 已按刚收到的 status packet sequence 写入，避免第一帧从本地 `0` 起步。
|
||||
3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`；但如果还没收到状态包，则会放行，后续命令仍要等第一帧 status 到达才会发出。
|
||||
4. 当前稠密轨迹结束不发送 `LastData=1`，而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。
|
||||
|
||||
序号和节拍已经按手册方向校准；停止语义仍需在真实 R30iB 联调中继续确认。
|
||||
|
||||
## 5. 命令缓冲
|
||||
|
||||
文档说明机器人可以缓冲提前到达的 command packet。默认启用，缓冲上限为 `$STMO.$PKT_STACK - 1`，`$PKT_STACK` 默认 `10`，可配置范围 `2..10`。`$STMO.$START_MOVE` 决定积累多少未处理命令包后开始运动，默认 `1`。
|
||||
|
||||
注意事项：
|
||||
|
||||
1. 只有 command packet 会进入缓冲。
|
||||
2. status output stop packet 会立即处理。
|
||||
3. 如果 command buffer 中还有未处理包，不应发送 status output stop packet。
|
||||
4. 使用 `Last data=1` 时，机器人会先处理完缓冲里的命令包，再结束外部控制。
|
||||
|
||||
当前实现没有显式预填 `$PKT_STACK` 缓冲，也没有读取 `$START_MOVE`。`FanucJ519Client` 只保存一个“当前命令”，由后台循环持续发送；`FanucControllerRuntime.SendDenseTrajectory()` 另一个 8ms 循环负责按轨迹时间更新这条当前命令。这与文档的“按状态包响应并可提前发多包缓冲”模型不同。
|
||||
|
||||
## 6. 可执行运动条件
|
||||
|
||||
文档列出的主要运动约束：
|
||||
|
||||
1. 目标点必须可达。
|
||||
2. 笛卡尔格式下目标点对应的关节解必须唯一，且 configuration 要与 `IBGN start` 开始时一致。
|
||||
3. 各轴必须满足上下限。
|
||||
4. 不能发生自碰撞。
|
||||
5. 必须考虑 FANUC J3 轴定义：J3 不是相对 J2 臂的夹角，而是机器人视角下相对水平面的 J3 臂角度。
|
||||
6. 外部设备必须控制每轴速度、加速度、jerk 不超过 `$STMO_GRP` 下的限制。
|
||||
7. 状态包中的当前位置是 servo feedback position，不是 command position。轨迹起点应平滑连接到机器人 command position，而不能简单用当前 servo position 直接起步。
|
||||
8. reducer load 超限也会导致停机，负载相关计算不公开。
|
||||
|
||||
当前实现对这些条件的覆盖：
|
||||
|
||||
| 条件 | 当前状态 |
|
||||
| --- | --- |
|
||||
| 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 |
|
||||
| `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 |
|
||||
| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` |
|
||||
| IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` |
|
||||
| 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 |
|
||||
| J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 |
|
||||
| command position 起步 | `MoveJoint` 会用当前运行时记录的关节作为起点生成 PTP 稠密轨迹；但没有通过 FANUC HMI 通信读取 command position |
|
||||
| reducer load | 未建模，依赖保守规划和现场报警反馈 |
|
||||
| 笛卡尔格式限制 | 运行时不走笛卡尔 J519 目标格式，暂不覆盖 configuration 变化报警 |
|
||||
|
||||
## 7. 系统变量
|
||||
|
||||
与本仓库后续最相关的变量：
|
||||
|
||||
| 变量 | 默认/含义 |
|
||||
| --- | --- |
|
||||
| `$STMO.$PHYS_PORT` | 物理口，`1=CD38A`，`2=CD38B` |
|
||||
| `$STMO.$COM_INT` | 通信周期，单位 ms，通常 `8`，只读 |
|
||||
| `$STMO.$PKT_STACK` | command buffer 最大保留量，默认 `10` |
|
||||
| `$STMO.$START_MOVE` | 缓冲中积累多少未处理命令后开始运动，默认 `1` |
|
||||
| `$STMO_GRP.$JNT_VEL_LIM[*]` | 各轴速度上限，degree/s，只读 |
|
||||
| `$STMO_GRP.$JNT_ACC_LIM[*]` | 各轴加速度上限，degree/s^2，只读 |
|
||||
| `$STMO_GRP.$JNT_JRK_LIM[*]` | 各轴 jerk 上限，degree/s^3，只读 |
|
||||
| `$STMO_GRP.$LMT_MODE` | 加速度/jerk 限制计算模式 |
|
||||
| `$STMO_GRP.$WARN_LIM` | 接近限制时报警阈值，默认 `80%` |
|
||||
| `$STMO_GRP.$FLTR_LN` | 命令目标移动平均滤波窗口 |
|
||||
| `$STMO_GRP.$MAX_SPD` | 用于限制计算的 flange center 最大速度 |
|
||||
|
||||
当前实现没有读取或设置上述系统变量。`RobotProfile.ServoPeriod` 当前决定运行时发送周期；对当前现场而言应继续确认它与 `$COM_INT` 一致。
|
||||
|
||||
## 8. 附录 B：加速度和 jerk 限制
|
||||
|
||||
文档说明，在 `$STMO_GRP[].$LMT_MODE=0` 时，加速度和 jerk 的允许上限会根据 flange center speed 与 payload 计算：
|
||||
|
||||
1. 以 `$MAX_SPD` 分成 20 档速度区间。
|
||||
2. 每个轴、每种限制类型都有无负载和最大负载两张 20 档表。
|
||||
3. 实际 payload 通过线性插值得到限制表。
|
||||
4. 实际 flange center speed 在相邻速度档之间线性插值。
|
||||
5. 限制值不是每个通信周期都更新，而是在超过 `Vmax/20` 到再次跌回阈值的整段时间内，以该段观测到的 `Vpeak` 决定。
|
||||
6. 如果长时间不跌回阈值，会按中间检查时间做临时判断。
|
||||
|
||||
文档还提供了 packet type `3` 的限制表查询协议：
|
||||
|
||||
| 包 | 方向 | 重点字段 |
|
||||
| --- | --- | --- |
|
||||
| 请求 | 外部设备 -> 机器人 | packet type `3`、version `1`、axis `1..9`、limit type `0=velocity/1=acceleration/2=jerk` |
|
||||
| 响应 | 机器人 -> 外部设备 | packet type `3`、version `1`、axis、limit type、`Vmax`、中间检查时间、无负载 20 档、最大负载 20 档 |
|
||||
|
||||
当前实现没有 packet type `3` 查询，也没有实现手册描述的动态限制表算法。现阶段规划时长和保守程度主要依赖 replacement 自身参数与现场抓包对齐。
|
||||
|
||||
## 9. 报警与诊断
|
||||
|
||||
文档中与实现最相关的报警：
|
||||
|
||||
| 报警 | 含义 |
|
||||
| --- | --- |
|
||||
| `MOTN-600` | 命令序号与机器人期望不一致 |
|
||||
| `MOTN-602` | data format 非法 |
|
||||
| `MOTN-603` | 后续命令包未在通信周期内到达 |
|
||||
| `MOTN-604` | 命令包过多，超出缓冲 |
|
||||
| `MOTN-605` | 目标位置包含 NaN 或 infinity |
|
||||
| `MOTN-606` | 非 AUTO 或 override 不是 100% |
|
||||
| `MOTN-607` | 协议版本不匹配 |
|
||||
| `MOTN-609/610/611` | 速度、加速度、jerk 超限 |
|
||||
| `MOTN-612/613/614` | 接近速度、加速度、jerk 限制 |
|
||||
| `MOTN-617` | 目标点与当前位置不连续 |
|
||||
| `MOTN-619` | 当前机型不支持笛卡尔目标格式 |
|
||||
| `PRIO-023` | 读写的 IO 类型或索引未分配 |
|
||||
|
||||
当前 `ControllerStateSnapshot.ActiveAlarms` 仍为空，Web 状态页也尚未接入 FANUC 报警列表。后续现场联调如果出现报警，应优先按上述表格关联 J519 包序号、目标数据、IO 字段、发送间隔和状态包 bit。
|
||||
|
||||
## 10. 与当前代码的结论
|
||||
|
||||
已基本对齐：
|
||||
|
||||
1. UDP 60015、大端、start/stop 控制包、64B command packet、132B status packet 的基础二进制布局。
|
||||
2. `Data format=1` 的关节目标下发。
|
||||
3. 状态位 bit0..bit3 的解析和快照暴露。
|
||||
4. 规划输出 `rad` 转 J519 `deg`。
|
||||
5. 根据 `speed_ratio` 做运行期时间轴缩放，而不是改变规划文件时间。
|
||||
6. 飞拍 IO 触发通过 command packet 的写 IO 字段下发。
|
||||
7. 命令发送按机器人 UDP status 包驱动，并使用最新 status sequence 回发。
|
||||
|
||||
主要差异/风险：
|
||||
|
||||
1. 当前未实现命令缓冲预填，也未读取 `$PKT_STACK / $START_MOVE`。
|
||||
2. 当前停止运动依赖 packet type `2` stop 控制包，没有稳定发送 `LastData=1` 的最后 command packet；这与手册标准结束语义不同。
|
||||
3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询，也未实现 payload/speed 20 档动态限制算法。
|
||||
4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。
|
||||
5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。
|
||||
|
||||
建议后续联调优先级：
|
||||
|
||||
1. 验证运动结束是否必须补 `LastData=1`；如果当前 stop 控制包能稳定工作，也应在文档中标为现场兼容路径，而不是手册标准路径。
|
||||
2. 抓一次报警现场包，确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。
|
||||
3. 如果后续追求更稳的真实机运行，补 packet type `3` 限制表查询，并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。
|
||||
591
docs/planning-duration-mismatch-investigation.md
Normal file
591
docs/planning-duration-mismatch-investigation.md
Normal file
@@ -0,0 +1,591 @@
|
||||
# 轨迹规划时长差异调查记录
|
||||
|
||||
## 背景
|
||||
|
||||
当前新 C# 规划链路在不额外缩放规划约束时，部分真实现场轨迹会比旧 RVBUST/FlyingShot 导出的 `JointTraj.txt` 更短。
|
||||
|
||||
最典型现象：
|
||||
|
||||
- 真实 `Rvbust/uttc-20260428/Data/JointTraj.txt`：`UTTC_MS11` 总时长约 `7.403046s`。
|
||||
- 新 C# 当前默认规划输出：`src/Flyshot.Server.Host/bin/Debug/net8.0/Config/Data/UTTC_MS11/JointTraj.txt` 总时长约 `5.495112s`。
|
||||
- 实体机复核确认：修改运行时 `speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长。
|
||||
|
||||
因此，本问题不应继续归因到运行时 `speed_ratio`，而应归到规划阶段的有效关节约束来源。
|
||||
|
||||
## 已确认事实
|
||||
|
||||
1. `speed_ratio` 是运行执行倍率。
|
||||
|
||||
UTTC 抓包和实体机测试都显示，`speed_ratio=0.7` 会拉伸 J519 实际下发时间和包数，但不会改变已生成的 `JointTraj.txt` 规划时间轴。
|
||||
|
||||
2. `JointTraj.txt` 是规划结果点位。
|
||||
|
||||
`saveTrajectory` / `SaveTrajInfo` / `IsFlyshotTrajectoryValid(saveTrajectory=true)` 生成的 `JointTraj.txt` 表示规划后的 sparse waypoint 时间轴，不是上传的原始飞拍路径，也不是 J519 逐周期下发点。
|
||||
|
||||
3. UTTC_MS11 的差异是整条时间轴等比例缩放。
|
||||
|
||||
对 `UTTC_MS11`，真实时间和当前 C# 默认规划时间之间的比例在所有 waypoint 上都一致：
|
||||
|
||||
```text
|
||||
C#默认规划时间 / 真实规划时间 = 5.495112 / 7.403046 = 0.742277
|
||||
```
|
||||
|
||||
这说明路点顺序、相对分段时间和 ICSP 主要逻辑基本一致，差异更像是规划时传入的有效 `vel/acc/jerk` joint limits 存在整体倍率差异。
|
||||
|
||||
4. 现场配置中没有找到显式倍率字段。
|
||||
|
||||
已检查现场现有配置，未发现类似 `planning_speed_scale` 或等价字段保存了 `0.742277`、`0.7`、`0.9` 等规划倍率值。
|
||||
|
||||
## 样本对比
|
||||
|
||||
| 样本 | 真实 `JointTraj.txt` 时长 | 当前/已有新规划时长 | 等效倍率 `新规划/真实` | 说明 |
|
||||
| --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `5.495112s` | `0.742277` | 所有 waypoint 时间均为同一比例 |
|
||||
| `UTTC_MS11_TEST01` | `7.805885s` | `5.814370s` | `0.744870` | `20260428 多点` 新增 1 个路径点后仍几乎是整条时间轴等比例缩放 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `0.706394` | 同样表现为新规划偏快 |
|
||||
| `EOL9_EAU_0` / `EOL9_EAU_90` | `6.400851s` | `5.651140s` | `0.882873` | `EOL9 EAU 0` 与 `EOL9 EAU 90` 的真实 `JointTraj.txt` 文件一致 |
|
||||
| `EOL9_EAU_90` | `6.400851s` | `6.471610s` | `1.011055` | 使用 `speedRatio=0.9 + self-adapt-icsp` 的旧离线结果已接近真实 |
|
||||
|
||||
这些样本说明差异不是 `UTTC_MS11` 的个案，也不是一个可以全局写死的常数。不同真实样本对应的等效规划倍率不同。
|
||||
|
||||
## `20260428 多点` 新样本对比
|
||||
|
||||
2026-04-30 追加现场新样本：
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/RobotConfig.json
|
||||
../Rvbust/20260428 多点/JointTraj.txt
|
||||
../Rvbust/20260428 多点/JointDetialTraj.txt
|
||||
```
|
||||
|
||||
该样本中的飞拍程序名为：
|
||||
|
||||
```text
|
||||
UTTC_MS11_TEST01
|
||||
```
|
||||
|
||||
配置摘要：
|
||||
|
||||
```text
|
||||
waypoints=21
|
||||
shot_flags=21
|
||||
acc_limit=1
|
||||
jerk_limit=1
|
||||
```
|
||||
|
||||
实机导出的 `JointTraj.txt`：
|
||||
|
||||
```text
|
||||
rows=21
|
||||
duration=7.805885s
|
||||
```
|
||||
|
||||
用当前 C# `ICspPlanner`、同一个 `LR_Mate_200iD_7L.robot`、同一份 `RobotConfig.json` 规划：
|
||||
|
||||
```text
|
||||
rows=21
|
||||
duration=5.814370s
|
||||
C# / 实机 = 0.744870
|
||||
```
|
||||
|
||||
逐点/逐段统计：
|
||||
|
||||
```text
|
||||
point_ratio_std = 2.18e-7
|
||||
segment_ratio_std = 9.0e-7
|
||||
max_joint_diff = 5.0e-7 rad
|
||||
```
|
||||
|
||||
这说明：
|
||||
|
||||
1. 新样本的路点关节值与 C# 输入基本完全一致，不是解析错点或单位错位。
|
||||
2. 新增 1 个路径点后，C# 与旧系统仍然保持几乎严格的整条时间轴等比例差异。
|
||||
3. `UTTC_MS11_TEST01` 的倍率 `0.744870` 与原 `UTTC_MS11` 的 `0.742277` 非常接近，进一步支持“同一类 UTTC 现场导出使用了一组更保守的 effective JointLimits”这一判断。
|
||||
|
||||
和原 `UTTC_MS11` 对比：
|
||||
|
||||
```text
|
||||
原 UTTC_MS11 实机 rows=20 duration=7.403046s
|
||||
新 UTTC_MS11_TEST01 实机 rows=21 duration=7.805885s
|
||||
新增路径点后实机时长增加 0.402839s
|
||||
```
|
||||
|
||||
当前观察不到新增点导致规划形状或局部段比例失真；它更像是在同一套旧系统规划约束下正常增加了一段路径时间。
|
||||
|
||||
## `20260430.pcap` 初始化抓包复核
|
||||
|
||||
2026-04-30 继续复核现场提供的完整初始化抓包：
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/20260430.pcap
|
||||
```
|
||||
|
||||
抓包总览：
|
||||
|
||||
```text
|
||||
packet_count=4821
|
||||
tcp_payload_bytes=35302
|
||||
udp_payload_bytes=451946
|
||||
```
|
||||
|
||||
主要有效负载会话为：
|
||||
|
||||
```text
|
||||
UDP 192.168.10.11:60015 -> 192.168.10.10:56118 260700B
|
||||
UDP 192.168.10.11:60015 -> 192.168.10.10:48455 127116B
|
||||
UDP 192.168.10.10:48455 -> 192.168.10.11:60015 62088B
|
||||
TCP 192.168.10.11:10010 -> 192.168.10.10:42106 35102B
|
||||
TCP 192.168.10.11:10012 -> 192.168.10.10:33528 106B
|
||||
TCP 192.168.10.10:33528 -> 192.168.10.11:10012 94B
|
||||
```
|
||||
|
||||
全包搜索以下明文关键字没有命中：
|
||||
|
||||
```text
|
||||
Joint / joint / Limit / limit / vel / acc / jerk / Speed / speed /
|
||||
Robot / JSON / Traj / ratio / Ratio / GetJoint / SetJoint
|
||||
```
|
||||
|
||||
`TCP 10012` 命令通道按 `doz + length + message_id + body + zod` 解码后，只看到以下初始化/程序命令：
|
||||
|
||||
| 方向 | 消息号 | 含义 | 请求体/结果 |
|
||||
| --- | ---: | --- | --- |
|
||||
| C->R | `0x0001` | 未知握手 | 空请求 |
|
||||
| R->C | `0x0001` | 未知握手响应 | `result=0` |
|
||||
| C->R | `0x2000` | 未知版本/状态查询 | 空请求 |
|
||||
| R->C | `0x2000` | 未知版本/状态响应 | 包含 `0.6.0` 字符串 |
|
||||
| C->R | `0x2100` | `ResetRobot` | 空请求 |
|
||||
| R->C | `0x2100` | `ResetRobot` 响应 | `result=0` |
|
||||
| C->R | `0x2003` | `GetProgramStatus("RVBUSTSM")` | 程序名 `RVBUSTSM` |
|
||||
| R->C | `0x2003` | 程序状态响应 | `result=0, status=1` |
|
||||
| C->R | `0x2102` | `StartProgram("RVBUSTSM")` | 程序名 `RVBUSTSM` |
|
||||
| R->C | `0x2102` | 启动响应 | `result=0` |
|
||||
|
||||
没有看到：
|
||||
|
||||
- `0x2207 SetSpeedRatio`
|
||||
- `0x2206 GetSpeedRatio`
|
||||
- `0x2200/0x2201 GetTcp/SetTcp`
|
||||
- `0x2208/0x2209 GetIo/SetIo`
|
||||
- 任何疑似 `JointLimits / velocity / acceleration / jerk` 的参数帧
|
||||
|
||||
`TCP 10010` 状态通道只有机器人侧到上位机的状态帧：
|
||||
|
||||
```text
|
||||
390 个 90B 状态帧
|
||||
1 个 2B 连接前导
|
||||
```
|
||||
|
||||
这些帧与已逆向的 `pose[6] + joint[6] + external_axes[3] + raw_tail_words[4]` 状态布局一致，不携带规划约束。
|
||||
|
||||
`UDP 60015` J519 通道只出现既有三类长度：
|
||||
|
||||
```text
|
||||
C->R 8B 初始化包 1 个
|
||||
C->R 64B 目标关节命令包 970 个
|
||||
R->C 132B 反馈包 2938 个
|
||||
```
|
||||
|
||||
没有出现其他长度的 UDP 参数帧。64B 命令包是 J519 逐周期目标关节/IO 命令，132B 是机器人反馈；这条链路承载的是执行期 streaming motion，而不是旧 RVBUST 规划器的 joint limit 配置。
|
||||
|
||||
阶段结论：
|
||||
|
||||
```text
|
||||
20260430.pcap 只覆盖机器人侧 10010 / 10012 / 60015 通信。
|
||||
它没有 50001/TCP+JSON，也没有 ControllerServer/Python 客户端到旧服务端的配置调用。
|
||||
因此，这份抓包看不到旧规划阶段的 effective JointLimits。
|
||||
```
|
||||
|
||||
这并不否定“旧系统规划瞬间存在更保守的 effective JointLimits”这一方向；它只说明这份初始化抓包不是抓取该信息的位置。若要抓到这类限制，需要抓旧服务端内部 `_GetJointLimits/_SetJointLimits`，或者抓上层 Python/GUI 与 ControllerServer 之间的配置/规划调用，而不是只抓机器人控制柜侧的执行链路。
|
||||
|
||||
## `all-50001.pcap` 本机 50001 抓包复核
|
||||
|
||||
2026-04-30 追加复核本机所有网卡抓包：
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/all-50001.pcap
|
||||
SHA256=C3543F314AE446CABA8E2097EFAFB36F39DD73FFE166F051A1F9387CFD15990F
|
||||
```
|
||||
|
||||
该文件由 `tcpdump -i any` 生成，pcap linktype 为 `113`，即 Linux cooked capture。按 SLL 头解析后，确认抓到了本机到本机的 `50001` TCP JSON 通信：
|
||||
|
||||
```text
|
||||
192.168.1.100:35814 -> 192.168.1.100:50001 217B payload
|
||||
192.168.1.100:50001 -> 192.168.1.100:35814 91B payload
|
||||
```
|
||||
|
||||
客户端到服务端的完整 JSON 命令序列为：
|
||||
|
||||
```json
|
||||
{"reply_from_client":true}
|
||||
{"cmd":"SetUpRobot","robot_name":"FANUC_LR_Mate_200iD_7L"}
|
||||
{"cmd":"IsSetUp"}
|
||||
{"cmd":"SetActiveController","sim":false}
|
||||
{"cmd":"Connect","ip":"192.168.10.11"}
|
||||
{"buffer_size":8,"cmd":"EnableRobot"}
|
||||
```
|
||||
|
||||
服务端返回为：
|
||||
|
||||
```json
|
||||
{"test_from_server": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
{"res": true}
|
||||
```
|
||||
|
||||
这说明本次抓包确实覆盖到了旧 `50001` 控制链路，但当前只包含机器人初始化、连接和使能流程。里面没有看到：
|
||||
|
||||
- `ExecuteFlyShotTraj`
|
||||
- `SaveTrajInfo`
|
||||
- `IsFlyShotTrajValid`
|
||||
- `GetJointLimits / SetJointLimits`
|
||||
- `SetVelocityLimit / SetAccelerationLimit / SetJerkLimit`
|
||||
- 任何包含 `acc_limit / jerk_limit / JointLimits / velocity / acceleration / jerk` 的配置 JSON
|
||||
|
||||
阶段结论：
|
||||
|
||||
```text
|
||||
all-50001.pcap 已经证明抓包接口选对了；
|
||||
但这次只抓到了初始化链路，没有抓到规划/保存轨迹那一刻的 50001 请求。
|
||||
```
|
||||
|
||||
该待确认点已由下一节 `all-50001-plan.pcap` 覆盖：后续抓包确实抓到了 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)`，仍未出现规划限制字段。
|
||||
|
||||
## `all-50001-plan.pcap` 规划执行抓包复核
|
||||
|
||||
2026-04-30 追加复核规划/执行动作期间的本机 50001 抓包：
|
||||
|
||||
```text
|
||||
../Rvbust/20260428 多点/all-50001-plan.pcap
|
||||
SHA256=311DC45B4789ED11EBEAB7A396E2EE7A16EC8534E20F10127FB43BBAD823C21D
|
||||
```
|
||||
|
||||
该抓包同样是 `tcpdump -i any` 生成的 Linux cooked capture，已按 SLL 头解析。有效 TCP JSON 流为：
|
||||
|
||||
```text
|
||||
192.168.1.100:35814 -> 192.168.1.100:50001 2612B payload
|
||||
192.168.1.100:50001 -> 192.168.1.100:35814 516B payload
|
||||
```
|
||||
|
||||
客户端到服务端的关键命令序列为：
|
||||
|
||||
```json
|
||||
{"cmd":"ListFlyShotTraj"}
|
||||
{"cmd":"GetNextListFlyShotTraj","count":0}
|
||||
{"cmd":"SetSpeedRatio","ratio":0.5}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"SetSpeedRatio","ratio":1.0}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"StartUploadFlyShotTraj","name":"UTTC_MS11"}
|
||||
{"cmd":"UploadFlyShotTraj", "...":"4 批共 20 个 waypoint，每批包含 waypoints / shot_flags / offset_values / addrs"}
|
||||
{"cmd":"EndUploadFlyShotTraj","name":"UTTC_MS11"}
|
||||
{"cmd":"ListFlyShotTraj"}
|
||||
{"cmd":"GetNextListFlyShotTraj","count":0}
|
||||
```
|
||||
|
||||
两次执行请求均为：
|
||||
|
||||
```json
|
||||
{
|
||||
"cmd": "ExecuteFlyShotTraj",
|
||||
"method": "icsp",
|
||||
"move_to_start": true,
|
||||
"name": "UTTC_MS11_TEST01",
|
||||
"save_traj": true,
|
||||
"use_cache": false,
|
||||
"wait": true
|
||||
}
|
||||
```
|
||||
|
||||
它们前面的速度倍率分别为：
|
||||
|
||||
```text
|
||||
第一次：SetSpeedRatio ratio=0.5
|
||||
第二次：SetSpeedRatio ratio=1.0
|
||||
```
|
||||
|
||||
服务端对所有命令均返回：
|
||||
|
||||
```json
|
||||
{"res": true}
|
||||
```
|
||||
|
||||
这份抓包确认了两点：
|
||||
|
||||
1. 公开 50001 JSON 链路确实会把 `SetSpeedRatio` 和 `ExecuteFlyShotTraj(save_traj=true,use_cache=false)` 发给旧服务端。
|
||||
2. 即便覆盖到了实际执行/保存轨迹动作，请求中仍没有出现 `GetJointLimits / SetJointLimits`、`SetVelocityLimit / SetAccelerationLimit / SetJerkLimit`，也没有 `acc_limit / jerk_limit / velocity / acceleration / jerk / JointLimits` 等规划限制字段。
|
||||
|
||||
因此，当前能从 50001 抓包确认的是：
|
||||
|
||||
```text
|
||||
规划方法、是否保存轨迹、是否使用缓存、是否等待执行，都会显式发到旧服务端；
|
||||
速度倍率通过 SetSpeedRatio 单独发到旧服务端；
|
||||
但 effective JointLimits 没有通过这次公开 50001 JSON 请求显式传入。
|
||||
```
|
||||
|
||||
这进一步收敛了差异来源：如果旧系统规划时确实使用了更保守的 joint limits，它更可能来自旧服务端在 `SetUpRobot("FANUC_LR_Mate_200iD_7L")` 后加载/初始化的内部状态，或来自 GUI/服务端内部私有路径，而不是这次 50001 公开 JSON 在 `ExecuteFlyShotTraj` 请求中传入的字段。
|
||||
|
||||
## Joint3/Joint2 couple A/B 测试
|
||||
|
||||
2026-04-30 追加测试：为了验证 `.robot` 中 `Joint3` 对 `Joint2` 的 couple 是否是规划时长差异主因，使用 Python ICSP demo 做了多组只读 A/B。
|
||||
|
||||
测试模型来自：
|
||||
|
||||
```text
|
||||
flyshot-replacement/Config/Models/LR_Mate_200iD_7L.robot
|
||||
```
|
||||
|
||||
其中 `Joint3` 的 couple 信息为：
|
||||
|
||||
```text
|
||||
q3_kin = q3_raw + q2_kin * 1.0 + 0.0
|
||||
```
|
||||
|
||||
测试变体：
|
||||
|
||||
- `raw`：原始 6 轴路点直接规划。
|
||||
- `replace_q3=q3+q2`：规划输入中把第 3 轴替换为耦合后的运动学角。
|
||||
- `replace_q3=q3-q2`：反向符号试探，排除符号理解错误。
|
||||
- `raw+constraint(q3+q2)`：保留原始 6 轴，同时追加虚拟约束轴 `q3+q2`，用 Joint3 的 `vel/acc/jerk` 限值检查。
|
||||
- `raw+constraint(q3-q2)`：反向符号的虚拟约束轴试探。
|
||||
|
||||
结果：
|
||||
|
||||
| 样本 | 真实时长 | 最接近变体 | 变体时长 | 变体/真实 | 与真实差值 | 结论 |
|
||||
| --- | ---: | --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `raw` | `5.495112s` | `0.742277` | `1.907934s` | couple 变体全部更短，且破坏原本严格等比例关系 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `replace_q3=q3+q2` | `10.600711s` | `0.713863` | `4.249077s` | couple 只改善约 `0.11s`，距离真实仍差 `4.25s` |
|
||||
| `EOL9_EAU_90` | `6.400851s` | `raw+constraint(q3+q2)` | `5.748560s` | `0.898093` | `0.652291s` | couple 约束有小幅影响，但仍不足以解释真实时长 |
|
||||
|
||||
关键观察：
|
||||
|
||||
1. `UTTC_MS11` 的 `raw` 规划时间和真实时间保持严格等比例，`point_ratio_std=0`、`segment_ratio_std≈0`；加入 couple 后反而出现分段比例波动。
|
||||
2. `EOL10_EAU_0` 与 `EOL9_EAU_90` 的 couple 变体只带来小幅时长变化，不能解释 10% 到 30% 级别的差异。
|
||||
3. 因此，当前证据不支持“只要把 Joint3/Joint2 couple 带入 ICSP，就能对齐旧 RVBUST 规划时长”。
|
||||
|
||||
阶段结论：
|
||||
|
||||
`Joint3` couple 确实是 C# 与 Python demo 当前都没有进入规划约束的缺口，但它不像本轮时长 mismatch 的主因。它更可能影响 FK/运动学边界或少数局部段约束；当前主要时长差异仍更像有效 joint limits、旧系统运行期规划倍率、或 RPS 内部 ICSP 参数来源不同。
|
||||
|
||||
## 同模型复核与更可能的差异层
|
||||
|
||||
2026-04-30 继续复核：
|
||||
|
||||
1. 当前仓库固化的模型与旧 `FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot` 字节哈希一致。
|
||||
2. `ControllerClientCompatRobotCatalog` 当前会把 `FANUC_LR_Mate_200iD` 和 `FANUC_LR_Mate_200iD_7L` 都映射到 `LR_Mate_200iD_7L.robot`。
|
||||
3. `LR_Mate_200iD.robot` 短臂模型的前三轴 `vel/acc/jerk` 比 `7L` 更高。用短臂模型试算会让轨迹更短，不会解释“旧系统真实导出更慢”。
|
||||
|
||||
模型 A/B：
|
||||
|
||||
| 样本 | 真实时长 | `LR_Mate_200iD_7L.robot` | `LR_Mate_200iD.robot` | 结论 |
|
||||
| --- | ---: | ---: | ---: | --- |
|
||||
| `UTTC_MS11` | `7.403046s` | `5.495112s` | `5.345600s` | 短臂模型方向更错 |
|
||||
| `EOL10_EAU_0` | `14.849788s` | `10.489800s` | `10.342456s` | 短臂模型方向更错 |
|
||||
|
||||
因此，如果现场确认机器人模型确实一致，差异层就不应继续放在 `.robot` 静态文件本身，而应放在旧服务端规划时的运行态：
|
||||
|
||||
- 服务端内部存在 `_GetJointLimits / _SetJointLimits`，说明规划消费的是一份可能被运行期覆写的 `current JointLimits`。
|
||||
- `ControllerClient.h` 的 `ExecuteFlyShotTraj(..., use_cache=false)` 明确说明旧服务端可以把计算好的轨迹保存在内存中并复用。
|
||||
- `SaveTrajInfo(name, method)` 没有 `use_cache` 参数，不能仅凭公开头文件判断它一定每次从当前配置重新规划。
|
||||
|
||||
当前更合理的解释是：
|
||||
|
||||
```text
|
||||
同一个 .robot
|
||||
-> SetUpRobot 初始化基础 JointLimits
|
||||
-> 旧服务端运行期间可能被 _SetJointLimits / 速度倍率联动 / 缓存轨迹 覆盖
|
||||
-> SaveTrajInfo 或 IsFlyShotTrajValid(save_traj=true) 导出的是真正规划时那份状态
|
||||
-> 当前 C# 每次用静态 .robot + RobotConfig 重新规划，所以时长更短
|
||||
```
|
||||
|
||||
尤其需要注意：`EOL10_EAU_0` 的 `新规划/真实` 比例为 `0.706394`，接近 `0.7`；`EOL9_EAU_90` 的比例为 `0.882873`，接近 `0.9`。这不像模型误差，更像历史导出时混入了某个运行态速度/限制倍率。`UTTC_MS11` 的 `0.742277` 不等于抓包确认的执行层 `0.7`，所以不能简单把所有样本都归因到 `SetSpeedRatio`，但“运行态规划约束不是静态模型值”仍是目前最强方向。
|
||||
|
||||
## 旧服务端与 GUI 二进制复核
|
||||
|
||||
2026-04-30 继续从旧系统二进制字符串中复核，重点看公开 Python/HTTP 层没有暴露出来的运行态对象。
|
||||
|
||||
### 服务端确实持有 runtime JointLimits
|
||||
|
||||
`../FlyingShot/FlyingShot/Python/ControllerServer/ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能稳定看到以下方法和关键字：
|
||||
|
||||
```text
|
||||
ControllerServer.ControllerServer._GetJointLimits
|
||||
ControllerServer.ControllerServer._SetJointLimits
|
||||
ControllerServer.ControllerServer._IsWaypointInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJerkLimits
|
||||
ControllerServer.ControllerServer._ExecuteFlyShotTraj
|
||||
ControllerServer.ControllerServer._SaveTrajInfo
|
||||
ControllerServer.ControllerServer._IsFlyShotTrajValid
|
||||
SetVelocityLimit
|
||||
SetAccelerationLimit
|
||||
SetJerkLimit
|
||||
GetMaxVelocity
|
||||
GetMaxAcceleration
|
||||
GetMaxJerk
|
||||
m_acc_limit
|
||||
m_jerk_limit
|
||||
save_traj_only
|
||||
use_cache
|
||||
```
|
||||
|
||||
这比公开 `ControllerClient.h` 暴露的信息更多。它说明旧服务端内部不是只把 `.robot` 静态值直接传给 `TrajectoryRnICSP`，而是存在一份可以查询、设置、校验、再用于规划的运行期 `JointLimits`。
|
||||
|
||||
### GUI 也直接接触规划约束与保存逻辑
|
||||
|
||||
旧 GUI 二进制里也能看到同一条链：
|
||||
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotManager.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `GetJointLimits`
|
||||
- `TrajectoryRnICSP`
|
||||
- `IsTrajInJointLimits`
|
||||
- `IsTrajInJerkLimits`
|
||||
- `acc_limit`
|
||||
- `jerk_limit`
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Robot/RobotConfig.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `SaveTraj`
|
||||
- `m_acc_limit`
|
||||
- `m_jerk_limit`
|
||||
- `../FlyingShot/FlyingShot/Python/GUI/Panels/FlyshotDockPanel.cpython-37m-x86_64-linux-gnu.so`
|
||||
- `__SaveTraj`
|
||||
- `IsTrajInJointLimits`
|
||||
- `IsTrajInJerkLimits`
|
||||
- `m_acc_limit`
|
||||
- `m_jerk_limit`
|
||||
|
||||
这说明旧 GUI 的“保存轨迹/检查轨迹”路径很可能不是简单调用公开 `ControllerClient.SaveTrajInfo` 后结束，而是直接拿当前 `JointLimits + acc_limit + jerk_limit` 做规划、合法性检查或保存。
|
||||
|
||||
### UAES 接口没有显式对齐 JointLimits
|
||||
|
||||
`../flyshot-uaes-interface/main.py` 中 `/execute_flyshot/` 的执行路径是：
|
||||
|
||||
```text
|
||||
c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
`/set_speedRatio/` 是单独接口：
|
||||
|
||||
```text
|
||||
c.SetSpeedRatio(speed)
|
||||
```
|
||||
|
||||
同时，`../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so` 和 `../flyshot-uaes-interface/lib/libControllerClient.so` 中只看到公开客户端侧的：
|
||||
|
||||
```text
|
||||
GetSpeedRatio
|
||||
SetSpeedRatio
|
||||
ExecuteFlyShotTraj
|
||||
SaveTrajInfo
|
||||
IsFlyShotTrajValid
|
||||
JointLimits
|
||||
```
|
||||
|
||||
没有看到客户端侧 `GetJointLimits / SetJointLimits` 符号。也就是说，UAES Python 服务本身大概率没有主动把旧服务端的 runtime JointLimits 设置成某个值；如果现场旧导出时的 limits 被改过，更可能来自：
|
||||
|
||||
- 旧 GUI 初始化/保存路径；
|
||||
- 旧服务端内部默认初始化；
|
||||
- 服务端隐藏 TCP JSON 方法；
|
||||
- 历史上某次执行/保存后留下的缓存结果。
|
||||
|
||||
### 样本文件与配置文件可能不是同一次运行态
|
||||
|
||||
新增一个需要警惕的现象：
|
||||
|
||||
- `../Rvbust/EOL9 EAU 0/eol9_eau_0.json` 中 `acc_limit=1`、`jerk_limit=1`。
|
||||
- `../Rvbust/EOL9 EAU 90/eol9_eau_90.json` 中 `acc_limit=0.8`、`jerk_limit=0.8`。
|
||||
- 但两个目录下保存的真实 `JointTraj.txt` 内容和时长一致。
|
||||
|
||||
哈希复核：
|
||||
|
||||
```text
|
||||
EOL9 EAU 0 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC
|
||||
EOL9 EAU 90 JointTraj.txt SHA256=DFD8E1130742CFB4ED72F70D0E8CA4E3A16F421E0D0D9D921B9F5177717536EC
|
||||
eol9_eau_0.json SHA256=354D0D3F71499951976504802C4B2860132D1E4FF753738715A500529CD0BB68
|
||||
eol9_eau_90.json SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE536E1B98DF981CD32B27
|
||||
```
|
||||
|
||||
这说明不能默认认为“某个 JSON 文件当前内容”就一定是旁边 `Data/JointTraj.txt` 的生成状态。旧系统的保存文件可能来自缓存、拷贝、历史运行态，或 GUI/服务端中未落盘到该 JSON 的当前 `JointLimits`。
|
||||
|
||||
本轮新增证据把方向进一步收敛为：
|
||||
|
||||
```text
|
||||
同一个 .robot 文件本身不是问题核心；
|
||||
真正影响时长的是旧系统规划瞬间的 effective JointLimits；
|
||||
但这份状态没有出现在现有配置、机器人侧抓包或 50001 公开 JSON 中。
|
||||
```
|
||||
|
||||
如果未来能直接进入旧服务端进程，仍可在 `SaveTrajInfo` / `IsFlyShotTrajValid(save_traj=true)` 前后抓取 `_GetJointLimits` 返回值，并把它与 `.robot` 原始 `vel/acc/jerk` 和当前 JSON 的 `acc_limit/jerk_limit` 做数值对比。但这不再阻塞 replacement 的现场对齐：当前设计默认用显式内部规划加速度参数补齐这份不可见状态。
|
||||
|
||||
## 当前判断
|
||||
|
||||
当前最可信的解释是：
|
||||
|
||||
1. 旧 RVBUST/FlyingShot 生成真实 `JointTraj.txt` 时，规划阶段使用的有效 joint limits 并不总是 `.robot` 文件中的原始 `velocity / acceleration / jerk`。
|
||||
2. 这些有效 joint limits 可能来自服务运行期状态，例如旧服务端内部的 `_SetJointLimits`、上层 GUI/脚本初始化流程、机器人环境配置，或其他未落入当前 JSON 文件的运行时参数。
|
||||
3. 现有现场 JSON 中只明确保存了：
|
||||
- `acc_limit`
|
||||
- `jerk_limit`
|
||||
- `adapt_icsp_try_num`
|
||||
- IO 相关配置
|
||||
4. 已重新抓取机器人侧 `10010/10012/60015` 和本机 `50001/TCP+JSON`，仍没有看到 `JointLimits / velocity / acceleration / jerk / acc_limit / jerk_limit` 通过公开链路在规划时下发。
|
||||
5. 目前没有证据表明现场配置文件或公开 TCP JSON 显式保存了一个“规划速度倍率”或“规划加速度限制”。
|
||||
|
||||
因此，`0.742277` 不应被理解为固定业务常量。它只是 `UTTC_MS11` 在当前 C# 默认约束和真实导出结果之间反推出来的等效规划倍率。
|
||||
|
||||
## 兼容设计决策
|
||||
|
||||
由于重新抓包后仍抓不到旧系统的 effective limits，新系统后续不再继续假设公开链路会传入这份数据，而是采用 replacement-only 的显式规划约束参数补齐不可见状态。
|
||||
|
||||
参数分层如下：
|
||||
|
||||
1. `acc_limit / jerk_limit`
|
||||
- 来源：旧 `RobotConfig.json` 中已经存在的字段。
|
||||
- 语义：继续作为旧配置的基础倍率，参与 `.robot` 模型加载。
|
||||
- 限制：现场样本中 `acc_limit=1`、`jerk_limit=1` 时，不能解释旧导出轨迹更慢的问题。
|
||||
|
||||
2. `planning_acceleration_scale`
|
||||
- 来源：新系统内部兼容参数，不声称来自旧 RVBUST 配置或抓包。
|
||||
- 语义：只用于规划阶段，额外缩放 `JointLimit.AccelerationLimit`，用于复现旧服务端不可见的保守加速度约束。
|
||||
- 默认值：`1.0`，表示不额外限制。
|
||||
- 现场校准：若按纯加速度限制解释 `UTTC_MS11_TEST01`，可先用 `(5.814370 / 7.805885)^2 ≈ 0.5548` 作为候选起点，再用真实 `JointTraj.txt` 对拍确认。
|
||||
|
||||
3. `planning_speed_scale`
|
||||
- 来源：当前 C# 已支持的显式兼容字段。
|
||||
- 语义：把整条规划时间轴按速度倍率解释，联动缩放速度、加速度、jerk。
|
||||
- 定位：保留为临时整体验证开关；当后续落地 `planning_acceleration_scale` 后，现场默认优先使用加速度限制参数，而不是把 `planning_speed_scale` 当成旧系统事实。
|
||||
|
||||
当前 C# 已支持的 `planning_speed_scale` 形式为：
|
||||
|
||||
```json
|
||||
{
|
||||
"robot": {
|
||||
"planning_speed_scale": 0.742277
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
该字段只用于规划阶段：
|
||||
|
||||
- `vel *= planning_speed_scale`
|
||||
- `acc *= planning_speed_scale^2`
|
||||
- `jerk *= planning_speed_scale^3`
|
||||
|
||||
它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按：
|
||||
|
||||
```text
|
||||
t_traj = k * 0.008 * speed_ratio
|
||||
```
|
||||
|
||||
从已生成轨迹中重采样。
|
||||
|
||||
由于现场真实配置和本轮抓包中都没有找到这类倍率，所有 `planning_*` 字段都必须标注为 replacement-only 兼容校准参数，不能声称它们来自旧配置文件或公开 TCP JSON。
|
||||
|
||||
## 后续设计方向
|
||||
|
||||
1. 默认不再把运行时 `speed_ratio` 混入 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 的规划时间计算。
|
||||
2. 后续实现优先新增 `planning_acceleration_scale`，只限制规划加速度，并将其写入 `RobotConfig.json` 的 `robot` 节点或当前现场默认配置。
|
||||
3. 若只需快速对齐整条时间轴，可临时使用现有 `planning_speed_scale`；但文档、日志和配置说明必须标注它是新系统校准值。
|
||||
4. 如果未来能直接调用旧服务端 `_GetJointLimits`，再用返回值替换当前反推参数；在此之前，显式内部参数是当前可控且可审计的兼容策略。
|
||||
202
docs/python-client-interface-reverse-engineering.md
Normal file
202
docs/python-client-interface-reverse-engineering.md
Normal file
@@ -0,0 +1,202 @@
|
||||
# Python ControllerClient 接口逆向记录
|
||||
|
||||
## 背景
|
||||
|
||||
本记录用于确认旧 `PyControllerClient` 对 Python 暴露了哪些接口，尤其确认是否能通过 Python client 直接查询或设置旧服务端运行态 `JointLimits`。
|
||||
|
||||
复核对象：
|
||||
|
||||
```text
|
||||
../flyshot-uaes-interface/lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
../FlyingShot/FlyingShot/Lib/PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
../flyshot-uaes-interface/lib/libControllerClient.so
|
||||
../FlyingShot/FlyingShot/Lib/libControllerClient.so
|
||||
../FlyingShot/FlyingShot/Include/ControllerClient/ControllerClient.h
|
||||
../FlyingShot/FlyingShot/Include/ControllerClient/Types.h
|
||||
../flyshot-uaes-interface/UseControllerClient.py
|
||||
../flyshot-uaes-interface/main.py
|
||||
```
|
||||
|
||||
两份 Python 扩展与两份底层 client 库哈希一致：
|
||||
|
||||
```text
|
||||
PyControllerClient.cpython-37m-x86_64-linux-gnu.so
|
||||
SHA256=648CC23CBC6DF83822B58AC4A10211EE1DF8029AD8933D31032187748DF7F4BC
|
||||
|
||||
libControllerClient.so
|
||||
SHA256=6D6FD3F20F0791F1CF11EEE5B1D479E2DCB6A1A2C8AB00A1165575BAB4B62813
|
||||
```
|
||||
|
||||
因此 `flyshot-uaes-interface/lib` 与 `FlyingShot/FlyingShot/Lib` 中的 Python client 可视为同一份接口。
|
||||
|
||||
## 暴露的 Python 类型
|
||||
|
||||
`PyControllerClient` 暴露以下类型：
|
||||
|
||||
| 类型 | 来源 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `ControllerClient` | `ControllerClient.h` | TCP JSON client，高层控制入口 |
|
||||
| `JointPositions` | `Types.h` | 关节位置容器，可用 6 维列表构造，也支持下标读写 |
|
||||
| `Pose` | `Types.h` | TCP/末端位姿容器，C++ 侧为 7 元数组 |
|
||||
| `JointLimits` | `Types.h` | 关节上下限、速度、加速度、jerk 容器 |
|
||||
| `IOType` | `Types.h` | IO 枚举 |
|
||||
|
||||
`IOType` 的枚举值：
|
||||
|
||||
```text
|
||||
IOType.kIOTypeDI = 1
|
||||
IOType.kIOTypeDO = 2
|
||||
IOType.kIOTypeRI = 8
|
||||
IOType.kIOTypeRO = 9
|
||||
```
|
||||
|
||||
## ControllerClient 暴露方法
|
||||
|
||||
二进制字符串和 C++ 公开头文件交叉确认，Python client 暴露的方法为：
|
||||
|
||||
| Python 方法 | 典型调用 | 返回形态 | 说明 |
|
||||
| --- | --- | --- | --- |
|
||||
| `ConnectServer` | `c.ConnectServer(server_ip="127.0.0.1", port=50001)` | `bool` | 连接旧 `50001/TCP+JSON` 服务端 |
|
||||
| `GetServerVersion` | `c.GetServerVersion()` | `str` | Python 包装层把 C++ out 参数折叠成返回值 |
|
||||
| `GetClientVersion` | `c.GetClientVersion()` | `str` | 获取 client 版本 |
|
||||
| `SetUpRobot` | `c.SetUpRobot("FANUC_LR_Mate_200iD")` | `bool` | 按机器人名称初始化服务端机器人模型 |
|
||||
| `SetUpRobotFromEnv` | `c.SetUpRobotFromEnv(env_file)` | `bool` | 从环境文件初始化 |
|
||||
| `IsSetUp` | `c.IsSetUp()` | `bool` | 判断服务端是否已经初始化机器人 |
|
||||
| `SetShowTCP` | `c.SetShowTCP(is_show=True, axis_length=0.1, axis_size=2)` | `bool` | 仿真显示 TCP 坐标系 |
|
||||
| `GetName` | `c.GetName()` | `str` | 获取机器人名称 |
|
||||
| `GetDoF` | `c.GetDoF()` | `int` | 获取自由度 |
|
||||
| `SetActiveController` | `c.SetActiveController(sim=True)` | `bool` | 切换仿真/真实控制器 |
|
||||
| `Connect` | `c.Connect("192.168.10.101")` | `bool` | 连接机器人控制器 |
|
||||
| `Disconnect` | `c.Disconnect()` | `bool` | 断开机器人控制器 |
|
||||
| `EnableRobot` | `c.EnableRobot()` / `c.EnableRobot(8)` | `bool` | 使能机器人，参数为 buffer size |
|
||||
| `DisableRobot` | `c.DisableRobot()` | `bool` | 下使能 |
|
||||
| `GetSpeedRatio` | `c.GetSpeedRatio()` | `float` | 获取执行速度倍率 |
|
||||
| `SetSpeedRatio` | `c.SetSpeedRatio(0.8)` | `bool` | 设置执行速度倍率 |
|
||||
| `GetTCP` | `res, tcp = c.GetTCP()` | `(bool, Pose)` | 获取 TCP |
|
||||
| `SetTCP` | `c.SetTCP(tcp)` | `bool` | 设置 TCP |
|
||||
| `GetIO` | `res, value = c.GetIO(port=1, io_type=IOType.kIOTypeDI)` | `(bool, bool)` | 读取 IO |
|
||||
| `SetIO` | `c.SetIO(port=1, value=True, io_type=IOType.kIOTypeDO)` | `bool` | 写 IO |
|
||||
| `StopMove` | `c.StopMove()` | `bool` | 停止运动 |
|
||||
| `GetJointPosition` | `res, joints = c.GetJointPosition()` | `(bool, JointPositions)` | 获取当前关节角 |
|
||||
| `GetPose` | `res, pose = c.GetPose()` | `(bool, Pose)` | 获取当前末端位姿 |
|
||||
| `GetNearestIK` | `res, ik = c.GetNearestIK(pose, joint_seed=joints)` | `(bool, JointPositions)` | 按 seed 求最近 IK |
|
||||
| `MoveJoint` | `c.MoveJoint(joint_positions)` | `bool` | 关节运动 |
|
||||
| `ExecuteTrajectory` | `c.ExecuteTrajectory(waypoints=[...], method="icsp", save_traj=True)` | `bool` | 执行普通轨迹 |
|
||||
| `UploadFlyShotTraj` | `c.UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs)` | `bool` | 上传飞拍轨迹 |
|
||||
| `DeleteFlyShotTraj` | `c.DeleteFlyShotTraj(name)` | `bool` | 删除飞拍轨迹 |
|
||||
| `ListFlyShotTraj` | `c.ListFlyShotTraj()` | `list[str]` | 列出已上传飞拍轨迹 |
|
||||
| `ExecuteFlyShotTraj` | `c.ExecuteFlyShotTraj(name, move_to_start=True, method="icsp", save_traj=True)` | `bool` | 执行飞拍轨迹 |
|
||||
| `SaveTrajInfo` | `c.SaveTrajInfo(name, method="icsp")` | `bool` | 保存规划结果到 `~/Rvbust/Data` |
|
||||
| `IsFlyShotTrajValid` | `valid, time = c.IsFlyShotTrajValid(name, method="icsp", save_traj=True)` | `(bool, float)` | 检查飞拍轨迹是否合法并返回规划时长 |
|
||||
|
||||
## 没有暴露的关键接口
|
||||
|
||||
本轮重点确认：Python client 暴露方法中没有看到：
|
||||
|
||||
```text
|
||||
GetJointLimits
|
||||
SetJointLimits
|
||||
_GetJointLimits
|
||||
_SetJointLimits
|
||||
```
|
||||
|
||||
虽然 `PyControllerClient` 绑定了 `JointLimits` 类型，并且 `libControllerClient.so` 中存在 `JointLimits` 的输出运算符符号，但公开 `ControllerClient` 方法表中没有任何接收或返回 `JointLimits` 的 client 入口。
|
||||
|
||||
这和旧服务端二进制不同。旧服务端 `ControllerServer.cpython-37m-x86_64-linux-gnu.so` 中能看到：
|
||||
|
||||
```text
|
||||
ControllerServer.ControllerServer._GetJointLimits
|
||||
ControllerServer.ControllerServer._SetJointLimits
|
||||
ControllerServer.ControllerServer._IsWaypointInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJointLimits
|
||||
ControllerServer.ControllerServer._IsTrajInJerkLimits
|
||||
```
|
||||
|
||||
因此当前判断是：
|
||||
|
||||
```text
|
||||
Python client 公开 API 不能直接抓 runtime JointLimits；
|
||||
runtime JointLimits 查询能力存在于旧服务端内部，而不是 PyControllerClient 公开接口中。
|
||||
```
|
||||
|
||||
## UAES Python 服务实际使用的接口
|
||||
|
||||
`../flyshot-uaes-interface/main.py` 只使用了公开 client 方法：
|
||||
|
||||
- `ConnectServer`
|
||||
- `SetUpRobot`
|
||||
- `IsSetUp`
|
||||
- `EnableRobot`
|
||||
- `DisableRobot`
|
||||
- `SetActiveController`
|
||||
- `Connect`
|
||||
- `GetName`
|
||||
- `GetServerVersion`
|
||||
- `GetDoF`
|
||||
- `GetSpeedRatio`
|
||||
- `SetTCP`
|
||||
- `GetTCP`
|
||||
- `SetIO`
|
||||
- `GetJointPosition`
|
||||
- `MoveJoint`
|
||||
- `ListFlyShotTraj`
|
||||
- `UploadFlyShotTraj`
|
||||
- `ExecuteFlyShotTraj`
|
||||
- `SetSpeedRatio`
|
||||
- `DeleteFlyShotTraj`
|
||||
- `GetPose`
|
||||
|
||||
其中 `/execute_flyshot/` 调用：
|
||||
|
||||
```text
|
||||
c.ExecuteFlyShotTraj(name=name, move_to_start=True, method="icsp", save_traj=True)
|
||||
```
|
||||
|
||||
`/set_speedRatio/` 调用：
|
||||
|
||||
```text
|
||||
c.SetSpeedRatio(speed)
|
||||
```
|
||||
|
||||
没有看到 UAES 服务通过 Python client 设置或查询 `JointLimits`。
|
||||
|
||||
2026-04-30 追加 `50001/TCP+JSON` 抓包复核后，这个判断进一步收敛。`all-50001-plan.pcap` 中已经抓到两次真实规划/执行请求：
|
||||
|
||||
```json
|
||||
{"cmd":"SetSpeedRatio","ratio":0.5}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
{"cmd":"SetSpeedRatio","ratio":1.0}
|
||||
{"cmd":"ExecuteFlyShotTraj","method":"icsp","move_to_start":true,"name":"UTTC_MS11_TEST01","save_traj":true,"use_cache":false,"wait":true}
|
||||
```
|
||||
|
||||
请求中仍没有 `JointLimits / acc_limit / jerk_limit / velocity / acceleration / jerk`。因此公开 Python client 与公开 50001 JSON 都没有把规划限制作为参数传给 `ExecuteFlyShotTraj`。
|
||||
|
||||
另外，`main.py` 的 `/execute_trajectory/` 中出现：
|
||||
|
||||
```text
|
||||
c.yrxm(waypoints=joint_positions, method='icsp', save_traj=True)
|
||||
```
|
||||
|
||||
`yrxm` 不在 `PyControllerClient` 暴露方法表中，按上下文应是 `ExecuteTrajectory` 的笔误；这条不影响飞拍主路径 `/execute_flyshot/`。
|
||||
|
||||
## 对当前时长差异调查的含义
|
||||
|
||||
如果要抓旧系统规划时使用的 effective `vel/acc/jerk`，优先级应调整为：
|
||||
|
||||
1. 在旧服务端进程内直接调用或插桩 `_GetJointLimits`。
|
||||
2. 或者逆向 `50001/TCP+JSON` 的 hidden command envelope，再尝试发送 `GetJointLimits` / `_GetJointLimits`。
|
||||
3. 不应指望现有 `PyControllerClient.ControllerClient` 直接提供 `GetJointLimits`。
|
||||
|
||||
如果短期内无法进入旧服务端内部，新系统不再继续等待这份不可见状态；设计上使用 replacement-only 的内部规划约束参数补齐，优先限制规划加速度，例如 `planning_acceleration_scale`。该参数必须标注为新系统校准值，不能写成旧 Python client 或旧 50001 JSON 的公开字段。
|
||||
|
||||
最小现场验证脚本可以先确认 Python client 暴露面：
|
||||
|
||||
```python
|
||||
from PyControllerClient import ControllerClient
|
||||
|
||||
c = ControllerClient()
|
||||
names = [x for x in dir(c) if "Limit" in x or "limit" in x]
|
||||
print(names)
|
||||
```
|
||||
|
||||
按当前二进制逆向，预期不会出现 `GetJointLimits` / `SetJointLimits`。
|
||||
@@ -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,46 @@
|
||||
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>
|
||||
/// 获取或设置运行配置根目录；为空时默认使用程序基目录下的 Config。
|
||||
/// </summary>
|
||||
public string? ConfigRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置旧父工作区根目录；仅用于测试或旧样本显式兼容。
|
||||
/// </summary>
|
||||
public string? WorkspaceRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 解析运行配置根目录，确保运行时默认不再依赖源码仓库位置。
|
||||
/// </summary>
|
||||
/// <returns>运行配置根目录的绝对路径。</returns>
|
||||
public string ResolveConfigRoot()
|
||||
{
|
||||
var root = string.IsNullOrWhiteSpace(ConfigRoot)
|
||||
? Path.Combine(AppContext.BaseDirectory, "Config")
|
||||
: ConfigRoot;
|
||||
|
||||
return Path.GetFullPath(root);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析显式配置的旧父工作区根目录；未配置时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>旧父工作区根目录的绝对路径，或 null。</returns>
|
||||
public string? ResolveLegacyWorkspaceRoot()
|
||||
{
|
||||
return string.IsNullOrWhiteSpace(WorkspaceRoot)
|
||||
? null
|
||||
: Path.GetFullPath(WorkspaceRoot);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
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> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
||||
{
|
||||
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.robot",
|
||||
["FANUC_LR_Mate_200iD_7L"] = "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 (!SupportedRobotModelFileMap.TryGetValue(robotName, out var modelFileName))
|
||||
{
|
||||
throw new InvalidOperationException($"Unsupported robot name: {robotName}");
|
||||
}
|
||||
|
||||
var modelPath = ResolveModelPath(modelFileName);
|
||||
return _robotModelLoader.LoadProfile(modelPath, accLimitScale, jerkLimitScale);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析机器人模型路径，运行目录 Config/Models 优先，旧父工作区只作为显式兼容入口。
|
||||
/// </summary>
|
||||
/// <param name="modelFileName">运行目录 Models 下的机器人模型文件名。</param>
|
||||
/// <returns>可传给 .robot 加载器的模型文件绝对路径。</returns>
|
||||
private string ResolveModelPath(string modelFileName)
|
||||
{
|
||||
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
|
||||
if (File.Exists(configModelPath))
|
||||
{
|
||||
return configModelPath;
|
||||
}
|
||||
|
||||
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
|
||||
if (legacyWorkspaceRoot is not null)
|
||||
{
|
||||
return Path.Combine(legacyWorkspaceRoot, "FlyingShot", "FlyingShot", "Models", modelFileName);
|
||||
}
|
||||
|
||||
return configModelPath;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,837 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
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 FlyshotTrajectoryArtifactWriter _artifactWriter;
|
||||
private readonly JsonFlyshotTrajectoryStore _trajectoryStore;
|
||||
private readonly ILogger<ControllerClientCompatService>? _logger;
|
||||
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="artifactWriter">saveTrajectory 规划结果点位导出器。</param>
|
||||
/// <param name="trajectoryStore">统一 RobotConfig.json 持久化存储；为空时按配置根目录创建默认实例。</param>
|
||||
/// <param name="logger">日志记录器；允许测试直接构造时传入 null。</param>
|
||||
public ControllerClientCompatService(
|
||||
ControllerClientCompatOptions options,
|
||||
ControllerClientCompatRobotCatalog robotCatalog,
|
||||
IControllerRuntime runtime,
|
||||
ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
|
||||
RobotConfigLoader configLoader,
|
||||
FlyshotTrajectoryArtifactWriter? artifactWriter = null,
|
||||
JsonFlyshotTrajectoryStore? trajectoryStore = null,
|
||||
ILogger<ControllerClientCompatService>? logger = null)
|
||||
{
|
||||
_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));
|
||||
_artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader());
|
||||
_trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ConnectServer 完成: {ServerIp}:{Port}", serverIp, 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)
|
||||
{
|
||||
_logger?.LogInformation("SetUpRobot 开始: robotName={RobotName}", 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;
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, 恢复轨迹数={TrajCount}",
|
||||
robotName,
|
||||
robotProfile.DegreesOfFreedom,
|
||||
robotSettings.AccLimitScale,
|
||||
robotSettings.JerkLimitScale,
|
||||
_uploadedTrajectories.Count);
|
||||
}
|
||||
|
||||
/// <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));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Connect(robotIp);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.Disconnect();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize);
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.EnableRobot(bufferSize);
|
||||
}
|
||||
_logger?.LogInformation("EnableRobot 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
_logger?.LogInformation("DisableRobot 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.DisableRobot();
|
||||
}
|
||||
_logger?.LogInformation("DisableRobot 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
_logger?.LogInformation("StopMove 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
_runtime.StopMove();
|
||||
}
|
||||
_logger?.LogInformation("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);
|
||||
|
||||
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
|
||||
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("MoveJoint 完成");
|
||||
}
|
||||
|
||||
/// <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));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectory 开始: 路点数={WaypointCount}, method={Method}, saveTraj={SaveTraj}",
|
||||
waypoints.Count, options.Method, options.SaveTrajectory);
|
||||
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
|
||||
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
|
||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
|
||||
bundle.Result.Method,
|
||||
bundle.Result.Duration.TotalSeconds,
|
||||
bundle.Result.IsValid,
|
||||
bundle.Result.DenseJointTrajectory?.Count ?? 0,
|
||||
planningSpeedScale);
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectory 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose()
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
return _runtime.GetPose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"UploadTrajectory 开始: name={Name}, waypoints={WaypointCount}, shotFlags={ShotCount}",
|
||||
trajectory.Name,
|
||||
trajectory.Waypoints.Count,
|
||||
trajectory.ShotFlags.Count(static f => f));
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
|
||||
}
|
||||
|
||||
/// <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));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
|
||||
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
EnsureRuntimeEnabled();
|
||||
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
if (trajectory.Waypoints.Count == 0)
|
||||
{
|
||||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||||
}
|
||||
|
||||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||||
var settings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
|
||||
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
|
||||
name,
|
||||
bundle.Result.Method,
|
||||
bundle.Result.Duration.TotalSeconds,
|
||||
bundle.Result.TriggerTimeline.Count,
|
||||
bundle.Result.UsedCache,
|
||||
settings.PlanningSpeedScale);
|
||||
|
||||
if (options.MoveToStart)
|
||||
{
|
||||
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
|
||||
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
|
||||
}
|
||||
|
||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||
if (options.Wait)
|
||||
{
|
||||
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人模型。</param>
|
||||
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
|
||||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
|
||||
{
|
||||
var currentJointPositions = _runtime.GetJointPositions();
|
||||
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
|
||||
EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));
|
||||
|
||||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||||
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
|
||||
_logger?.LogInformation(
|
||||
"{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
|
||||
operationName,
|
||||
speedRatio,
|
||||
moveResult.Duration.TotalSeconds,
|
||||
moveResult.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
_runtime.ExecuteTrajectory(moveResult, targetJointPositions);
|
||||
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
|
||||
/// </summary>
|
||||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||||
/// <param name="plannedDuration">规划运动时长。</param>
|
||||
private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration)
|
||||
{
|
||||
var timeout = ResolveMotionCompletionTimeout(plannedDuration);
|
||||
var deadline = DateTimeOffset.UtcNow.Add(timeout);
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (!_runtime.GetSnapshot().IsInMotion)
|
||||
{
|
||||
_logger?.LogInformation("{OperationName} 运动完成", operationName);
|
||||
return;
|
||||
}
|
||||
|
||||
if (DateTimeOffset.UtcNow >= deadline)
|
||||
{
|
||||
throw new TimeoutException($"{operationName} 等待运动完成超时，planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。");
|
||||
}
|
||||
|
||||
Thread.Sleep(TimeSpan.FromMilliseconds(10));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。
|
||||
/// </summary>
|
||||
/// <param name="plannedDuration">规划运动时长。</param>
|
||||
/// <returns>等待运行时完成的最大时长。</returns>
|
||||
private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
|
||||
{
|
||||
var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
|
||||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
|
||||
var planningSettings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(saveTrajectory: true, method: method),
|
||||
planningSettings,
|
||||
planningSettings.PlanningSpeedScale);
|
||||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
var robot = RequireActiveRobot();
|
||||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||
{
|
||||
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||
}
|
||||
|
||||
var planningSettings = RequireRobotSettings();
|
||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||
robot,
|
||||
trajectory,
|
||||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||||
planningSettings,
|
||||
planningSettings.PlanningSpeedScale);
|
||||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
|
||||
|
||||
duration = bundle.Result.Duration;
|
||||
_logger?.LogInformation(
|
||||
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
|
||||
name, bundle.Result.IsValid, duration.TotalSeconds);
|
||||
return bundle.Result.IsValid;
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DeleteTrajectory(string name)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
if (!_uploadedTrajectories.Remove(name))
|
||||
{
|
||||
_logger?.LogWarning("DeleteTrajectory 失败: 轨迹不存在 name={Name}", name);
|
||||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||||
}
|
||||
|
||||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||
_trajectoryStore.Delete(robotName, name);
|
||||
}
|
||||
|
||||
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", 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>
|
||||
/// 校验关节向量与当前机器人自由度一致，且所有值都是有限数值。
|
||||
/// </summary>
|
||||
/// <param name="joints">待校验关节向量，单位为弧度。</param>
|
||||
/// <param name="expectedCount">期望自由度。</param>
|
||||
/// <param name="paramName">调用方参数名。</param>
|
||||
private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
|
||||
{
|
||||
if (joints.Count != expectedCount)
|
||||
{
|
||||
throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
|
||||
}
|
||||
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
var value = joints[index];
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||||
/// </summary>
|
||||
/// <param name="name">飞拍轨迹名称。</param>
|
||||
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
|
||||
/// <param name="robot">当前机器人模型。</param>
|
||||
/// <param name="bundle">规划结果包。</param>
|
||||
private void ExportFlyshotArtifactsIfRequested(
|
||||
string name,
|
||||
bool saveTrajectory,
|
||||
RobotProfile robot,
|
||||
PlannedExecutionBundle bundle)
|
||||
{
|
||||
if (!saveTrajectory)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置；失败时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>加载到的机器人配置，或 null。</returns>
|
||||
private CompatibilityRobotSettings? TryLoadRobotSettings()
|
||||
{
|
||||
foreach (var root in EnumerateRobotConfigRoots())
|
||||
{
|
||||
try
|
||||
{
|
||||
// 运行配置根本身已经是 Config 目录，这里用绝对路径避免再次追加 Config。
|
||||
var configPath = Path.Combine(root, "RobotConfig.json");
|
||||
var loaded = _configLoader.Load(configPath, root);
|
||||
return loaded.Robot;
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 单个候选根目录加载失败时继续尝试下一个兼容入口。
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 枚举 RobotConfig.json 的配置根目录，运行目录 Config 优先，旧父工作区仅在显式配置时参与。
|
||||
/// </summary>
|
||||
/// <returns>待尝试的配置根目录列表。</returns>
|
||||
private IEnumerable<string> EnumerateRobotConfigRoots()
|
||||
{
|
||||
yield return _options.ResolveConfigRoot();
|
||||
|
||||
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
|
||||
if (legacyWorkspaceRoot is not null)
|
||||
{
|
||||
yield return legacyWorkspaceRoot;
|
||||
}
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
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<FlyshotTrajectoryArtifactWriter>();
|
||||
services.AddSingleton<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,380 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
private readonly ICspPlanner _icspPlanner;
|
||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化轨迹编排器。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public ControllerClientTrajectoryOrchestrator(ILogger<ControllerClientTrajectoryOrchestrator>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
_icspPlanner = new(logger: null);
|
||||
_selfAdaptIcspPlanner = new(logger: null);
|
||||
}
|
||||
|
||||
/// <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,
|
||||
double planningSpeedScale = 1.0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
|
||||
waypoints.Count, options.Method, planningSpeedScale);
|
||||
|
||||
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: planningRobot,
|
||||
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);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
||||
result.Duration.TotalSeconds,
|
||||
result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
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,
|
||||
double? planningSpeedScale = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
settings ??= CreateDefaultRobotSettings();
|
||||
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}",
|
||||
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale);
|
||||
|
||||
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(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||||
return new PlannedExecutionBundle(
|
||||
cachedBundle.PlannedTrajectory,
|
||||
cachedBundle.ShotTimeline,
|
||||
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
|
||||
}
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: planningRobot,
|
||||
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: planningRobot.ServoPeriod,
|
||||
useDo: settings.UseDo);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
|
||||
uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
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,
|
||||
double planningSpeedScale)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
hash.Add(planningSpeedScale);
|
||||
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 limit in robot.JointLimits)
|
||||
{
|
||||
hash.Add(limit.JointName, StringComparer.Ordinal);
|
||||
hash.Add(limit.VelocityLimit);
|
||||
hash.Add(limit.AccelerationLimit);
|
||||
hash.Add(limit.JerkLimit);
|
||||
}
|
||||
|
||||
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>
|
||||
/// 按规划全局速度倍率生成规划专用机器人约束。
|
||||
/// </summary>
|
||||
/// <param name="robot">原始机器人约束。</param>
|
||||
/// <param name="planningSpeedScale">规划阶段的全局速度倍率，1.0 表示不额外缩放。</param>
|
||||
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
|
||||
private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
|
||||
{
|
||||
if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
|
||||
{
|
||||
return robot;
|
||||
}
|
||||
|
||||
// RVBUST 规划阶段会用独立限速倍率缩放有效限制；运行时 speedRatio 仍只负责 J519 下发重采样。
|
||||
var scaledLimits = robot.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * planningSpeedScale,
|
||||
limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
|
||||
limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
|
||||
.ToArray();
|
||||
|
||||
return new RobotProfile(
|
||||
name: robot.Name,
|
||||
modelPath: robot.ModelPath,
|
||||
degreesOfFreedom: robot.DegreesOfFreedom,
|
||||
jointLimits: scaledLimits,
|
||||
jointCouplings: robot.JointCouplings,
|
||||
servoPeriod: robot.ServoPeriod,
|
||||
triggerPeriod: robot.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <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,22 @@
|
||||
<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>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,54 @@
|
||||
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>
|
||||
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>
|
||||
public FlyshotExecutionOptions(
|
||||
bool moveToStart = true,
|
||||
string method = "icsp",
|
||||
bool saveTrajectory = true,
|
||||
bool useCache = true,
|
||||
bool wait = true)
|
||||
{
|
||||
MoveToStart = moveToStart;
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
UseCache = useCache;
|
||||
Wait = wait;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取执行前是否自动移动到轨迹起点。
|
||||
/// </summary>
|
||||
public bool MoveToStart { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否优先复用已规划轨迹缓存。
|
||||
/// </summary>
|
||||
public bool UseCache { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否等待机器人执行完整条飞拍轨迹后再返回。
|
||||
/// </summary>
|
||||
public bool Wait { get; }
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Export;
|
||||
using Flyshot.Core.Planning.Kinematics;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
|
||||
/// </summary>
|
||||
public sealed class FlyshotTrajectoryArtifactWriter
|
||||
{
|
||||
/// <summary>
|
||||
/// 旧 Data 明细点位文件使用的默认采样周期，单位为秒。
|
||||
/// </summary>
|
||||
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化规划结果点位导出器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||||
/// <param name="robotModelLoader">机器人模型加载器，用于生成笛卡尔点位。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FlyshotTrajectoryArtifactWriter(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader,
|
||||
ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将飞拍规划结果导出到 Config/Data/name。
|
||||
/// </summary>
|
||||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="bundle">规划结果包。</param>
|
||||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(bundle);
|
||||
|
||||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||||
Directory.CreateDirectory(outputDir);
|
||||
|
||||
// 明细文件对齐旧 Data 目录的 16ms 采样；运行时 J519 仍可使用自己的 8ms 伺服采样。
|
||||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||||
var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory);
|
||||
var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
bundle.PlannedTrajectory,
|
||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
||||
var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel);
|
||||
var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory(
|
||||
bundle.PlannedTrajectory,
|
||||
kinematicsModel,
|
||||
samplePeriod: LegacyDetailSamplePeriodSeconds);
|
||||
|
||||
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
|
||||
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
|
||||
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
|
||||
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
|
||||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}",
|
||||
trajectoryName,
|
||||
outputDir,
|
||||
jointTrajectory.Count,
|
||||
jointDetailTrajectory.Count);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 JointTraj.txt 行数据，格式为 time + 关节弧度。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var row = new List<double>(trajectory.PlannedWaypoints[index].Positions.Count + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 CartTraj.txt 行数据，格式为 time + x/y/z/qx/qy/qz/qw。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
|
||||
PlannedTrajectory trajectory,
|
||||
RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray());
|
||||
var row = new List<double>(pose.Length + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。
|
||||
/// </summary>
|
||||
private static string SanitizeDirectoryName(string name)
|
||||
{
|
||||
var invalidChars = Path.GetInvalidFileNameChars();
|
||||
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
|
||||
return new string(chars);
|
||||
}
|
||||
}
|
||||
223
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
223
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
@@ -0,0 +1,223 @@
|
||||
using System.Text.Json;
|
||||
using System.Text.Json.Nodes;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置。
|
||||
/// </summary>
|
||||
public sealed class JsonFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
private readonly ILogger<JsonFlyshotTrajectoryStore>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化基于 JSON 文件的轨迹存储。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader, ILogger<JsonFlyshotTrajectoryStore>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将单条轨迹持久化到统一 RobotConfig.json，同时更新机器人配置段。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(settings);
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
|
||||
robotName,
|
||||
trajectory.Name,
|
||||
trajectory.Waypoints.Count);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
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));
|
||||
|
||||
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
var existingJson = File.ReadAllText(path);
|
||||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||||
if (root is null)
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||||
{
|
||||
var removed = flyingShotsObj.Remove(trajectoryName);
|
||||
if (removed)
|
||||
{
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
_logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载统一 RobotConfig.json 中的所有轨迹，并回传机器人配置。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="settings">输出 RobotConfig.json 中的机器人配置；若文件不存在或解析失败则为 null。</param>
|
||||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path);
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 正在加载: {Path}", path);
|
||||
|
||||
var loaded = _configLoader.Load(path, _options.ResolveConfigRoot());
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
|
||||
robotName,
|
||||
dict.Count,
|
||||
settings?.UseDo,
|
||||
settings?.IoKeepCycles);
|
||||
|
||||
return dict;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path);
|
||||
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()
|
||||
{
|
||||
return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json");
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
@@ -0,0 +1,200 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
internal static class MoveJointTrajectoryGenerator
|
||||
{
|
||||
private const double BaseMoveJointDurationSeconds = 0.320;
|
||||
private const double VelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double AccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double JerkShapeCoefficient = 36.12609273600853;
|
||||
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||
|
||||
private static readonly double[] CapturedMvpointAlpha =
|
||||
[
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
];
|
||||
|
||||
public static TrajectoryResult CreateResult(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double speedRatio,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(startJoints);
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
if (startJoints.Count != robot.DegreesOfFreedom || targetJoints.Count != robot.DegreesOfFreedom)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
|
||||
}
|
||||
|
||||
var requestedDurationSeconds = ResolveDurationSeconds(robot, startJoints, targetJoints);
|
||||
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds * speedRatio;
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
logger?.LogDebug(
|
||||
"MoveJointTrajectoryGenerator: 请求时长={RequestedDuration:F4}s, 对齐后时长={Duration:F4}s, speedRatio={SpeedRatio}, 采样周期={SamplePeriod:F6}s, 采样数={SampleCount}",
|
||||
requestedDurationSeconds, durationSeconds, speedRatio, samplePeriodSeconds, denseJointTrajectory.Count);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
method: PlanningMethod.Doubles,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 2,
|
||||
plannedWaypointCount: denseJointTrajectory.Count,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
internal static double ResolveDurationSeconds(RobotProfile robot, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = BaseMoveJointDurationSeconds;
|
||||
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * VelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * AccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * JerkShapeCoefficient / limit.JerkLimit);
|
||||
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return Math.Max(1, intervals) * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(rawIntervals - 1e-9);
|
||||
if (intervals < 1 || intervals + 1 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double durationSeconds,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||||
|
||||
for (var index = 0L; index < sampleCount; index++)
|
||||
{
|
||||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||||
rows.Add(CreateRow(time, durationSeconds, startJoints, targetJoints));
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> CreateRow(double timeSeconds, double durationSeconds, IReadOnlyList<double> startJoints, IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var alpha = InterpolateCapturedAlpha(u);
|
||||
var row = new double[startJoints.Count + 1];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < startJoints.Count; index++)
|
||||
{
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * alpha);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
internal static double InterpolateCapturedAlpha(double normalizedTime)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var scaledIndex = clamped * (CapturedMvpointAlpha.Length - 1);
|
||||
var lower = (int)Math.Floor(scaledIndex);
|
||||
var upper = Math.Min(lower + 1, CapturedMvpointAlpha.Length - 1);
|
||||
var fraction = scaledIndex - lower;
|
||||
|
||||
return CapturedMvpointAlpha[lower]
|
||||
+ ((CapturedMvpointAlpha[upper] - CapturedMvpointAlpha[lower]) * fraction);
|
||||
}
|
||||
}
|
||||
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; }
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -22,7 +22,7 @@ public enum CompatibilityPathStyle
|
||||
public static class PathCompatibility
|
||||
{
|
||||
/// <summary>
|
||||
/// 按旧系统常见目录约定解析配置文件路径。
|
||||
/// 按当前服务配置目录约定解析配置文件路径。
|
||||
/// </summary>
|
||||
/// <param name="configPath">调用方传入的原始配置路径。</param>
|
||||
/// <param name="repoRoot">当前兼容搜索的仓库根目录。</param>
|
||||
@@ -48,11 +48,10 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
var normalizedRepoRoot = Path.GetFullPath(repoRoot);
|
||||
var fileName = Path.GetFileName(rawPath);
|
||||
var checkedPaths = new List<string>();
|
||||
|
||||
// 先按最常见的候选路径顺序尝试，保持与旧工具链相近的定位逻辑。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath, fileName))
|
||||
// 相对路径只允许落在当前服务根目录的 Config 下，避免隐式回退到父工作区旧文件。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath))
|
||||
{
|
||||
var fullCandidate = Path.GetFullPath(candidate);
|
||||
if (checkedPaths.Contains(fullCandidate, StringComparer.OrdinalIgnoreCase))
|
||||
@@ -67,18 +66,6 @@ public static class PathCompatibility
|
||||
}
|
||||
}
|
||||
|
||||
// 最后一层兜底按文件名全仓库搜索，但只接受唯一命中，避免同名配置误判。
|
||||
var matches = Directory
|
||||
.EnumerateFiles(normalizedRepoRoot, fileName, SearchOption.AllDirectories)
|
||||
.Select(Path.GetFullPath)
|
||||
.Distinct(StringComparer.OrdinalIgnoreCase)
|
||||
.ToArray();
|
||||
|
||||
if (matches.Length == 1)
|
||||
{
|
||||
return matches[0];
|
||||
}
|
||||
|
||||
throw new FileNotFoundException(
|
||||
$"未找到配置文件 '{configPath}'。已检查: {string.Join(", ", checkedPaths)}",
|
||||
configPath);
|
||||
@@ -106,15 +93,11 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 枚举旧系统中最常见的配置候选路径。
|
||||
/// 枚举当前服务配置目录下允许的配置候选路径。
|
||||
/// </summary>
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath, string fileName)
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath)
|
||||
{
|
||||
yield return Path.Combine(repoRoot, rawPath);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", "Data", fileName);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", "Install", "FlyingShot", "Config", fileName);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", fileName);
|
||||
yield return Path.Combine(repoRoot, fileName);
|
||||
yield return Path.Combine(repoRoot, "Config", rawPath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
@@ -17,7 +18,8 @@ public sealed class CompatibilityRobotSettings
|
||||
int ioKeepCycles,
|
||||
double accLimitScale,
|
||||
double jerkLimitScale,
|
||||
int adaptIcspTryNum)
|
||||
int adaptIcspTryNum,
|
||||
double planningSpeedScale = 1.0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(ioAddresses);
|
||||
|
||||
@@ -36,6 +38,11 @@ public sealed class CompatibilityRobotSettings
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
if (planningSpeedScale <= 0.0 || double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (adaptIcspTryNum < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
|
||||
@@ -54,6 +61,7 @@ public sealed class CompatibilityRobotSettings
|
||||
AccLimitScale = accLimitScale;
|
||||
JerkLimitScale = jerkLimitScale;
|
||||
AdaptIcspTryNum = adaptIcspTryNum;
|
||||
PlanningSpeedScale = planningSpeedScale;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -81,6 +89,11 @@ public sealed class CompatibilityRobotSettings
|
||||
/// </summary>
|
||||
public double JerkLimitScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划阶段的全局速度倍率，只影响 JointTraj 基准时间，不等同于运行时 J519 下发速度倍率。
|
||||
/// </summary>
|
||||
public double PlanningSpeedScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取自适应补点最大尝试次数。
|
||||
/// </summary>
|
||||
@@ -131,6 +144,17 @@ public sealed class LoadedRobotConfig
|
||||
/// </summary>
|
||||
public sealed class RobotConfigLoader
|
||||
{
|
||||
private readonly ILogger<RobotConfigLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 RobotConfigLoader。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public RobotConfigLoader(ILogger<RobotConfigLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载一份旧版 RobotConfig.json。
|
||||
/// </summary>
|
||||
@@ -139,6 +163,8 @@ public sealed class RobotConfigLoader
|
||||
/// <returns>规范化后的配置文档。</returns>
|
||||
public LoadedRobotConfig Load(string configPath, string? repoRoot = null)
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 开始加载: configPath={ConfigPath}, repoRoot={RepoRoot}", configPath, repoRoot);
|
||||
|
||||
var resolvedRepoRoot = ResolveRepoRoot(repoRoot);
|
||||
var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot);
|
||||
|
||||
@@ -153,7 +179,8 @@ public sealed class RobotConfigLoader
|
||||
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
|
||||
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
|
||||
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0));
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0),
|
||||
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0));
|
||||
|
||||
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
||||
foreach (var programElement in flyingShotsElement.EnumerateObject())
|
||||
@@ -163,6 +190,10 @@ public sealed class RobotConfigLoader
|
||||
programs.Add(programName, program);
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
||||
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.AdaptIcspTryNum, programs.Count);
|
||||
|
||||
return new LoadedRobotConfig(
|
||||
sourcePath: resolvedConfigPath,
|
||||
robot: robot,
|
||||
@@ -253,7 +284,7 @@ public sealed class RobotConfigLoader
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 推断仓库根目录，优先使用调用方显式传入的值。
|
||||
/// 推断当前 replacement 仓库根目录，优先使用调用方显式传入的值。
|
||||
/// </summary>
|
||||
private static string ResolveRepoRoot(string? repoRoot)
|
||||
{
|
||||
@@ -267,7 +298,7 @@ public sealed class RobotConfigLoader
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
@@ -10,6 +11,16 @@ namespace Flyshot.Core.Config;
|
||||
public sealed class RobotModelLoader
|
||||
{
|
||||
private const uint JsonChunkType = 0x4E4F534A;
|
||||
private readonly ILogger<RobotModelLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 RobotModelLoader。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
@@ -35,6 +46,8 @@ public sealed class RobotModelLoader
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
@@ -76,6 +89,10 @@ public sealed class RobotModelLoader
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}",
|
||||
profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath);
|
||||
|
||||
return new RobotProfile(
|
||||
name: profileName,
|
||||
modelPath: resolvedModelPath,
|
||||
@@ -156,6 +173,8 @@ public sealed class RobotModelLoader
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", modelPath);
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
@@ -203,6 +222,8 @@ public sealed class RobotModelLoader
|
||||
coupleOffset: coupleOffset));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);
|
||||
|
||||
return new RobotKinematicsModel(name: profileName, joints: joints);
|
||||
}
|
||||
|
||||
|
||||
@@ -18,7 +18,14 @@ 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,
|
||||
byte? j519Status = null,
|
||||
uint? j519Sequence = null,
|
||||
bool? j519AcceptsCommand = null,
|
||||
bool? j519ReceivedCommand = null,
|
||||
bool? j519SystemReady = null,
|
||||
bool? j519RobotInMotion = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -34,6 +41,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 +51,13 @@ public sealed class ControllerStateSnapshot
|
||||
JointPositions = copiedJointPositions;
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
J519Status = j519Status;
|
||||
J519Sequence = j519Sequence;
|
||||
J519AcceptsCommand = j519AcceptsCommand;
|
||||
J519ReceivedCommand = j519ReceivedCommand;
|
||||
J519SystemReady = j519SystemReady;
|
||||
J519RobotInMotion = j519RobotInMotion;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -92,4 +107,46 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("activeAlarms")]
|
||||
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 10010 状态帧尾部原始状态字，仅用于诊断，不直接推断运行语义。
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应的原始状态字节；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Status")]
|
||||
public byte? J519Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应序号；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Sequence")]
|
||||
public uint? J519Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 accept_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519AcceptsCommand")]
|
||||
public bool? J519AcceptsCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 received_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519ReceivedCommand")]
|
||||
public bool? J519ReceivedCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 sysrdy 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519SystemReady")]
|
||||
public bool? J519SystemReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 rbt_inmotion 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519RobotInMotion")]
|
||||
public bool? J519RobotInMotion { 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>
|
||||
|
||||
@@ -15,12 +15,28 @@ namespace Flyshot.Core.Planning.Export;
|
||||
/// </summary>
|
||||
public static class TrajectoryExporter
|
||||
{
|
||||
/// <summary>
|
||||
/// 导出规划关节轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出稠密关节轨迹到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出规划笛卡尔轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteCartesianTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -28,7 +44,7 @@ public static class TrajectoryExporter
|
||||
/// </summary>
|
||||
public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -53,7 +69,7 @@ public static class TrajectoryExporter
|
||||
File.WriteAllText(path, json, new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
private static void WriteRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
|
||||
@@ -6,6 +6,10 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
@@ -24,6 +25,54 @@ 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;
|
||||
private readonly ILogger<ICspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance,
|
||||
ILogger<ICspPlanner>? logger = null)
|
||||
{
|
||||
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;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
@@ -37,9 +86,22 @@ public sealed class ICspPlanner
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划开始: 名称={Name}, 路点数={WaypointCount}, 自由度={Dof}, threshold={Threshold}, maxIterations={MaxIterations}",
|
||||
request.Program.Name, waypoints.Count, request.Robot.DegreesOfFreedom, _threshold, _maxIterations);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 输入路点: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Positions.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
var qs = WaypointsToArray(waypoints);
|
||||
var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"ICSP 约束限值: vel=[{Vel}], acc=[{Acc}], jerk=[{Jerk}]",
|
||||
string.Join(", ", velLimits.Select(v => v.ToString("F2"))),
|
||||
string.Join(", ", accLimits.Select(a => a.ToString("F2"))),
|
||||
string.Join(", ", jerkLimits.Select(j => j.ToString("F2"))));
|
||||
|
||||
// 初始段时长直接取相邻示教点的关节空间欧氏距离。
|
||||
var segmentDurations = ComputeInitialDurations(qs);
|
||||
int nseg = segmentDurations.Length;
|
||||
@@ -52,7 +114,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,8 +151,11 @@ public sealed class ICspPlanner
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < DefaultThreshold)
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
_logger?.LogDebug(
|
||||
"ICSP 第 {Iteration} 轮收敛: threshold={CurrentThreshold:E6}",
|
||||
iteration + 1, currentThreshold);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -105,6 +170,25 @@ public sealed class ICspPlanner
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
_logger?.LogError(
|
||||
"ICSP 规划未收敛: global_scale={GlobalScale:F6} > {Tolerance:F6}, 段缩放=[{Scales}]",
|
||||
globalScale, 1.0 + _finalScaleTolerance,
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划完成: 名称={Name}, 迭代轮数={Iterations}, 收敛阈值={Threshold:E6}, 总时长={Duration:F4}s, global_scale={GlobalScale:F6}",
|
||||
request.Program.Name, bestIterations, bestThreshold, bestWaypointTimes[^1], globalScale);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 段时长: [{Durations}], 段缩放: [{Scales}]",
|
||||
string.Join(", ", bestDurations.Select(d => d.ToString("F4"))),
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
@@ -8,7 +9,7 @@ namespace Flyshot.Core.Planning;
|
||||
/// 为什么需要这层？
|
||||
/// ---
|
||||
/// 逆向分析已经指出：原系统里普通 icsp 若仍有段 scale > 1，不会直接返回未收敛结果；
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把“超限段统一插入中点后再重规划”的逻辑显式落地，
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把"超限段统一插入中点后再重规划"的逻辑显式落地，
|
||||
/// 补上 demo 缺失的失败恢复路径。
|
||||
///
|
||||
/// 补点策略：
|
||||
@@ -24,7 +25,18 @@ public sealed class SelfAdaptIcspPlanner
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner = new();
|
||||
private readonly ICspPlanner _innerPlanner;
|
||||
private readonly ILogger<SelfAdaptIcspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 SelfAdaptIcspPlanner。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public SelfAdaptIcspPlanner(ILogger<SelfAdaptIcspPlanner>? logger = null)
|
||||
{
|
||||
_innerPlanner = new ICspPlanner(enforceFinalScale: false, logger: null);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
|
||||
@@ -48,6 +60,10 @@ public sealed class SelfAdaptIcspPlanner
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划开始: 名称={Name}, 原始路点数={WaypointCount}, 最大补点次数={MaxAttempts}",
|
||||
request.Program.Name, originalWaypointCount, adaptIcspTryNum);
|
||||
|
||||
var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
var currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
@@ -76,6 +92,9 @@ public sealed class SelfAdaptIcspPlanner
|
||||
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划完成: 名称={Name}, 补点轮数={Attempts}, 最终路点数={WaypointCount}, 迭代次数={Iterations}, 总时长={Duration:F4}s",
|
||||
request.Program.Name, attempt, currentWaypoints.Length, trajectory.Iterations, trajectory.WaypointTimes[^1]);
|
||||
// 所有段都满足约束，收敛成功。返回包含补中点后路点的轨迹。
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
@@ -89,15 +108,24 @@ public sealed class SelfAdaptIcspPlanner
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
"SelfAdaptICSP 第 {Attempt} 轮存在超限段: 超限段数={BadCount}, 段索引=[{Segments}], 最大缩放={MaxScale:F4}",
|
||||
attempt, badSegments.Count, string.Join(", ", badSegments), trajectory.SegmentScales.Max());
|
||||
|
||||
if (attempt >= maxAttempts)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// 对超限段插入中点，并同步扩展 shot 元数据。
|
||||
int waypointCountBefore = currentWaypoints.Length;
|
||||
(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
|
||||
InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"SelfAdaptICSP 补中点: 路点数 {Before} -> {After}, 插入段=[{Segments}]",
|
||||
waypointCountBefore, currentWaypoints.Length, string.Join(", ", badSegments));
|
||||
|
||||
currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
@@ -109,6 +137,9 @@ public sealed class SelfAdaptIcspPlanner
|
||||
}
|
||||
|
||||
double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
|
||||
_logger?.LogError(
|
||||
"SelfAdaptICSP 规划失败: 名称={Name}, 在 {Attempts} 轮补点后仍未收敛, 最大段缩放因子={MaxScale:F6}",
|
||||
request.Program.Name, adaptIcspTryNum, maxScale);
|
||||
throw new InvalidOperationException(
|
||||
$"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛，最大段缩放因子={maxScale:F6}。");
|
||||
}
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Triggering;
|
||||
|
||||
@@ -10,13 +11,17 @@ namespace Flyshot.Core.Triggering;
|
||||
public sealed class ShotTimelineBuilder
|
||||
{
|
||||
private readonly WaypointTimestampResolver _resolver;
|
||||
private readonly ILogger<ShotTimelineBuilder>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ShotTimelineBuilder，依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
|
||||
/// </summary>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver)
|
||||
/// <param name="resolver">时间戳解析器。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger<ShotTimelineBuilder>? logger = null)
|
||||
{
|
||||
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -25,8 +30,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,14 +75,25 @@ public sealed class ShotTimelineBuilder
|
||||
sampleTime: sampleTime,
|
||||
addressGroup: addressGroup));
|
||||
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
if (useDo)
|
||||
{
|
||||
// use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ShotTimeline 构建完成: shotFlags总数={ShotFlagCount}, 触发事件数={TriggerCount}, useDo={UseDo}, holdCycles={HoldCycles}",
|
||||
program.ShotFlags.Count(static f => f),
|
||||
triggerTimeline.Count,
|
||||
useDo,
|
||||
holdCycles);
|
||||
|
||||
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);
|
||||
}
|
||||
952
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
952
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
@@ -0,0 +1,952 @@
|
||||
using System.Diagnostics;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
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 readonly ILogger<FanucControllerRuntime>? _logger;
|
||||
|
||||
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>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public FanucControllerRuntime(ILogger<FanucControllerRuntime>? logger = null)
|
||||
{
|
||||
_commandClient = new FanucCommandClient();
|
||||
_stateClient = new FanucStateClient();
|
||||
_j519Client = new FanucJ519Client();
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 供测试注入 mock 客户端的内部构造函数。
|
||||
/// </summary>
|
||||
internal FanucControllerRuntime(FanucCommandClient commandClient, FanucStateClient stateClient, FanucJ519Client j519Client, ILogger<FanucControllerRuntime>? logger = null)
|
||||
{
|
||||
_commandClient = commandClient;
|
||||
_stateClient = stateClient;
|
||||
_j519Client = j519Client;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
if (string.IsNullOrWhiteSpace(robotName))
|
||||
{
|
||||
throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("ResetRobot: robotName={RobotName}, dof={Dof}", robotName, robot.DegreesOfFreedom);
|
||||
|
||||
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)
|
||||
{
|
||||
_logger?.LogInformation("SetActiveController: sim={Sim}", 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));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}, 仿真={IsSim}", robotIp, _activeControllerIsSimulation);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureActiveControllerSelected();
|
||||
if (_activeControllerIsSimulation!.Value)
|
||||
{
|
||||
_connectedRobotIp = robotIp;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
_logger?.LogInformation("Connect 完成(仿真): robotIp={RobotIp}", robotIp);
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("Connect 完成(真机): robotIp={RobotIp}, 三条通道已建立", robotIp);
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
_logger?.LogInformation("Disconnect 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
DisconnectClients();
|
||||
_connectedRobotIp = null;
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
_logger?.LogInformation("Disconnect 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
if (bufferSize <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}, 仿真={IsSim}", bufferSize, _activeControllerIsSimulation);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
_bufferSize = bufferSize;
|
||||
|
||||
if (IsSimulationMode)
|
||||
{
|
||||
_isEnabled = true;
|
||||
_logger?.LogInformation("EnableRobot 完成(仿真)");
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("EnableRobot 完成(真机): RVBUSTSM 已启动, J519 运动循环已开启");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
_logger?.LogInformation("DisableRobot 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_isEnabled = false;
|
||||
_isInMotion = false;
|
||||
}
|
||||
_logger?.LogInformation("DisableRobot 完成");
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
_logger?.LogInformation("StopMove 开始");
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
CancelSendTaskLocked();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_isInMotion = false;
|
||||
}
|
||||
_logger?.LogInformation("StopMove 完成");
|
||||
}
|
||||
|
||||
/// <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 必须是有限数值。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SetSpeedRatio: ratio={Ratio}", ratio);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_speedRatio = clampedRatio;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("SetSpeedRatio 完成: clampedRatio={ClampedRatio}", _speedRatio);
|
||||
}
|
||||
|
||||
/// <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?.JointRadians.Count >= _jointPositions.Length)
|
||||
{
|
||||
return frame.JointRadians.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>();
|
||||
byte? j519Status = null;
|
||||
uint? j519Sequence = null;
|
||||
bool? j519AcceptsCommand = null;
|
||||
bool? j519ReceivedCommand = null;
|
||||
bool? j519SystemReady = null;
|
||||
bool? j519RobotInMotion = null;
|
||||
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame is not null)
|
||||
{
|
||||
if (frame.JointRadians.Count >= jointPositions.Length)
|
||||
{
|
||||
jointPositions = frame.JointRadians.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;
|
||||
j519Status = j519Response.Status;
|
||||
j519Sequence = j519Response.Sequence;
|
||||
j519AcceptsCommand = j519Response.AcceptsCommand;
|
||||
j519ReceivedCommand = j519Response.ReceivedCommand;
|
||||
j519SystemReady = j519Response.SystemReady;
|
||||
j519RobotInMotion = 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,
|
||||
j519Status: j519Status,
|
||||
j519Sequence: j519Sequence,
|
||||
j519AcceptsCommand: j519AcceptsCommand,
|
||||
j519ReceivedCommand: j519ReceivedCommand,
|
||||
j519SystemReady: j519SystemReady,
|
||||
j519RobotInMotion: j519RobotInMotion);
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(result);
|
||||
ArgumentNullException.ThrowIfNull(finalJointPositions);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}",
|
||||
result.ProgramName, result.Method, result.Duration.TotalSeconds,
|
||||
result.DenseJointTrajectory is not null, result.TriggerTimeline.Count, _speedRatio);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureEnabled();
|
||||
EnsureValidTrajectory(result);
|
||||
EnsureJointCount(finalJointPositions.Count);
|
||||
CancelSendTaskLocked();
|
||||
|
||||
if (!IsSimulationMode && result.DenseJointTrajectory is not null)
|
||||
{
|
||||
if (_speedRatio <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
|
||||
}
|
||||
|
||||
EnsureJ519ReadyForDenseExecution();
|
||||
|
||||
// 真机模式且存在稠密路点：启动后台高精度发送任务。
|
||||
_isInMotion = true;
|
||||
_sendCts = new CancellationTokenSource();
|
||||
var ct = _sendCts.Token;
|
||||
_sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct);
|
||||
_logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务");
|
||||
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;
|
||||
_logger?.LogInformation("ExecuteTrajectory 完成(单点模式)");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放运行时持有的所有 Socket 客户端。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
CancelSendTaskLocked();
|
||||
DisconnectClients();
|
||||
_commandClient.Dispose();
|
||||
_stateClient.Dispose();
|
||||
_j519Client.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。
|
||||
/// </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 speedRatio = _speedRatio;
|
||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||
var triggerToleranceSeconds = trajectoryStepSeconds / 2.0;
|
||||
var durationSeconds = result.Duration.TotalSeconds;
|
||||
var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds);
|
||||
var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}",
|
||||
result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count);
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
long nextTick = stopwatch.ElapsedTicks;
|
||||
ushort ioValue = 0;
|
||||
ushort ioMask = 0;
|
||||
int holdRemaining = -1;
|
||||
int segmentIndex = 0;
|
||||
long logInterval = Math.Max(1, sampleCount / 10);
|
||||
int triggerFiredCount = 0;
|
||||
|
||||
try
|
||||
{
|
||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
nextTick += periodTicks;
|
||||
|
||||
var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds);
|
||||
var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex);
|
||||
|
||||
// 递减 IO 保持计数器；若已到期则清零。
|
||||
var clearMaskAfterSend = false;
|
||||
if (holdRemaining > 0)
|
||||
{
|
||||
holdRemaining--;
|
||||
}
|
||||
else if (holdRemaining == 0)
|
||||
{
|
||||
ioValue = 0;
|
||||
holdRemaining = -1;
|
||||
clearMaskAfterSend = true;
|
||||
}
|
||||
|
||||
// 检查当前周期是否有新的触发事件。
|
||||
if (holdRemaining < 0 && !clearMaskAfterSend)
|
||||
{
|
||||
foreach (var trigger in triggers)
|
||||
{
|
||||
if (Math.Abs(trajectoryTime - trigger.TriggerTime) <= triggerToleranceSeconds)
|
||||
{
|
||||
ioMask = ComputeIoValue(trigger.AddressGroup);
|
||||
ioValue = ioMask;
|
||||
holdRemaining = Math.Max(trigger.HoldCycles - 1, 0);
|
||||
triggerFiredCount++;
|
||||
_logger?.LogInformation(
|
||||
"J519 IO触发: time={Time:F4}s, addr=[{Addr}], holdCycles={HoldCycles}",
|
||||
trajectoryTime, string.Join(",", trigger.AddressGroup.Addresses), trigger.HoldCycles);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var command = new FanucJ519Command(
|
||||
sequence: 0,
|
||||
targetJoints: joints,
|
||||
writeIoType: 2,
|
||||
writeIoIndex: 1,
|
||||
writeIoMask: ioMask,
|
||||
writeIoValue: ioValue);
|
||||
|
||||
_j519Client.UpdateCommand(command);
|
||||
|
||||
if (clearMaskAfterSend)
|
||||
{
|
||||
ioMask = 0;
|
||||
}
|
||||
|
||||
// 周期性记录进度（Debug 级别，避免高频 Info 日志）。
|
||||
if (sampleIndex > 0 && sampleIndex % logInterval == 0)
|
||||
{
|
||||
_logger?.LogDebug(
|
||||
"SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), time={Time:F4}s",
|
||||
(int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, trajectoryTime);
|
||||
}
|
||||
|
||||
// 高精度忙等待直到下一伺服周期。
|
||||
while (stopwatch.ElapsedTicks < nextTick)
|
||||
{
|
||||
Thread.SpinWait(1);
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 实际耗时={ElapsedMs}ms",
|
||||
sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger?.LogWarning(
|
||||
"SendDenseTrajectory 被取消: 已完成 {Percent}% ({Current}/{Total}), 触发次数={TriggerFiredCount}",
|
||||
(int)((double)(sampleCount > 0 ? 0 : 0) / sampleCount * 100), 0, sampleCount, triggerFiredCount);
|
||||
// 正常取消，轨迹被中断。
|
||||
}
|
||||
finally
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发包数。
|
||||
/// </summary>
|
||||
private static long CalculateDenseSendSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
||||
{
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "Trajectory duration must be non-negative.");
|
||||
}
|
||||
|
||||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for dense J519 execution.");
|
||||
}
|
||||
|
||||
return (long)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 要求的角度制关节目标。
|
||||
/// </summary>
|
||||
private static double[] SampleDenseJointTrajectoryDegrees(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseTrajectory,
|
||||
double trajectoryTime,
|
||||
ref int segmentIndex)
|
||||
{
|
||||
if (denseTrajectory.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("Dense joint trajectory is empty.");
|
||||
}
|
||||
|
||||
if (denseTrajectory.Count == 1 || trajectoryTime <= denseTrajectory[0][0])
|
||||
{
|
||||
return denseTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = denseTrajectory.Count - 1;
|
||||
if (trajectoryTime >= denseTrajectory[lastIndex][0])
|
||||
{
|
||||
return denseTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && denseTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = denseTrajectory[segmentIndex];
|
||||
var end = denseTrajectory[segmentIndex + 1];
|
||||
var startTime = start[0];
|
||||
var endTime = end[0];
|
||||
var segmentDuration = endTime - startTime;
|
||||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
||||
|
||||
var jointCount = start.Count - 1;
|
||||
var joints = new double[jointCount];
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
var radians = start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha);
|
||||
joints[index] = RadiansToDegrees(radians);
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 若已有 J519 响应，则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。
|
||||
/// </summary>
|
||||
private void EnsureJ519ReadyForDenseExecution()
|
||||
{
|
||||
var response = _j519Client.GetLatestResponse();
|
||||
if (response is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (response.AcceptsCommand && response.SystemReady)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
"J519 status is not ready for dense execution: "
|
||||
+ $"accept_cmd={response.AcceptsCommand}, "
|
||||
+ $"received_cmd={response.ReceivedCommand}, "
|
||||
+ $"sysrdy={response.SystemReady}, "
|
||||
+ $"rbt_inmotion={response.RobotInMotion}, "
|
||||
+ $"seq={response.Sequence}, "
|
||||
+ $"status=0x{response.Status:X2}.");
|
||||
}
|
||||
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
/// <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)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
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.");
|
||||
}
|
||||
}
|
||||
}
|
||||
24
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
24
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
@@ -0,0 +1,24 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<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>
|
||||
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
@@ -0,0 +1,454 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
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 readonly ILogger<FanucCommandClient>? _logger;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC 命令通道客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FanucCommandClient(ILogger<FanucCommandClient>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <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。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("CommandClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_tcpClient = new TcpClient { NoDelay = true };
|
||||
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_stream = _tcpClient.GetStream();
|
||||
|
||||
_logger?.LogInformation("CommandClient 已连接: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开命令通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("CommandClient Disconnect");
|
||||
|
||||
_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 async Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StopProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StopProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient ResetRobot");
|
||||
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient ResetRobot 成功");
|
||||
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)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient GetProgramStatus: {ProgramName}", programName);
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient GetProgramStatus 成功: {ProgramName}", programName);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StartProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StartProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio");
|
||||
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio 成功: ratio={Ratio}", result.Ratio);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetSpeedRatio: ratio={Ratio}", ratio);
|
||||
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetSpeedRatio 成功: ratio={Ratio}", ratio);
|
||||
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)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetTcp: tcpId={TcpId}", tcpId);
|
||||
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetTcp 成功: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", result.Pose.Select(v => v.ToString("F2"))));
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <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)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetTcp: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", pose.Take(3).Select(v => v.ToString("F2"))));
|
||||
var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetTcp 成功: tcpId={TcpId}", tcpId);
|
||||
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)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetIo: port={Port}, ioType={IoType}", port, ioType);
|
||||
var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetIo 成功: port={Port}, value={Value}", port, result.Value);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <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)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetIo: port={Port}, value={Value}, ioType={IoType}", port, value, ioType);
|
||||
var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetIo 成功: port={Port}, value={Value}", port, value);
|
||||
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);
|
||||
var response = await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient 发送帧成功: 帧长度={FrameLength}, 响应长度={ResponseLength}", frame.Length, response.Length);
|
||||
return response;
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验普通命令响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验程序状态响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验速度倍率响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 TCP 位姿响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 IO 读取响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucIoResponse EnsureSuccess(FanucIoResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
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);
|
||||
}
|
||||
}
|
||||
372
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
372
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
@@ -0,0 +1,372 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
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 readonly ILogger<FanucJ519Client>? _logger;
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _cts;
|
||||
private Task? _receiveTask;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private bool _motionStarted;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已创建 UDP 套接字。
|
||||
/// </summary>
|
||||
public bool IsConnected => _udpClient is not null;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC J519 客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <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。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_udpClient = new UdpClient();
|
||||
_udpClient.Connect(ip, port);
|
||||
|
||||
// 发送初始化包。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 初始化包已发送");
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
_receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。
|
||||
/// </summary>
|
||||
public void StartMotion()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道未连接。");
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
if (_motionStarted)
|
||||
{
|
||||
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||
return;
|
||||
}
|
||||
|
||||
_motionStarted = true;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送状态输出停止包并停止 J519 命令发送。
|
||||
/// </summary>
|
||||
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
|
||||
lock (_commandLock)
|
||||
{
|
||||
_motionStarted = false;
|
||||
}
|
||||
|
||||
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 原子更新下一周期要发送的 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="command">新的 J519 命令。</param>
|
||||
public void UpdateCommand(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = command;
|
||||
_commandHistoryForTests?.Add(command);
|
||||
}
|
||||
|
||||
_logger?.LogDebug(
|
||||
"J519 UpdateCommand: joints=[{Joints}], ioMask=0x{IoMask:X4}, ioValue=0x{IoValue:X4}",
|
||||
string.Join(", ", command.TargetJoints.Select(j => j.ToString("F2"))),
|
||||
command.WriteIoMask,
|
||||
command.WriteIoValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
|
||||
/// </summary>
|
||||
internal void EnableCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_commandHistoryForTests = [];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取测试记录的命令历史。
|
||||
/// </summary>
|
||||
/// <returns>命令历史快照。</returns>
|
||||
internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
|
||||
}
|
||||
}
|
||||
|
||||
/// <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
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_receiveTask?.Dispose();
|
||||
_receiveTask = null;
|
||||
|
||||
_cts?.Dispose();
|
||||
_cts = null;
|
||||
|
||||
_udpClient?.Dispose();
|
||||
_udpClient = null;
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_commandHistoryForTests = null;
|
||||
_motionStarted = false;
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
}
|
||||
|
||||
_receiveTask?.Dispose();
|
||||
_cts?.Dispose();
|
||||
_udpClient?.Dispose();
|
||||
}
|
||||
|
||||
private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence)
|
||||
{
|
||||
return new FanucJ519Command(
|
||||
sequence,
|
||||
source.TargetJoints,
|
||||
source.LastData,
|
||||
source.ReadIoType,
|
||||
source.ReadIoIndex,
|
||||
source.ReadIoMask,
|
||||
source.DataStyle,
|
||||
source.WriteIoType,
|
||||
source.WriteIoIndex,
|
||||
source.WriteIoMask,
|
||||
source.WriteIoValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台接收循环：持续接收 132B 响应并解析。
|
||||
/// </summary>
|
||||
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ReceiveLoop 启动");
|
||||
long receiveCount = 0;
|
||||
FanucJ519Response? lastLoggedResponse = null;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
receiveCount++;
|
||||
await SendCommandForStatusAsync(response, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
// 仅在状态变化时记录 Info，避免高频日志。
|
||||
if (lastLoggedResponse is null
|
||||
|| lastLoggedResponse.Status != response.Status
|
||||
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
||||
|| lastLoggedResponse.SystemReady != response.SystemReady)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, sysrdy={SysRdy}, motion={Motion}, pose=[{Pose}], joints=[{Joints}]",
|
||||
response.Status,
|
||||
response.Sequence,
|
||||
response.AcceptsCommand,
|
||||
response.SystemReady,
|
||||
response.RobotInMotion,
|
||||
string.Join(", ", response.Pose.Select(v => v.ToString("F1"))),
|
||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F2"))));
|
||||
lastLoggedResponse = response;
|
||||
}
|
||||
else if (receiveCount % 1000 == 0)
|
||||
{
|
||||
_logger?.LogDebug("J519 已接收 {Count} 个响应包", receiveCount);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 正常取消，共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="response">刚收到的状态包。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
private async Task SendCommandForStatusAsync(FanucJ519Response response, CancellationToken cancellationToken)
|
||||
{
|
||||
var udpClient = _udpClient;
|
||||
if (udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
FanucJ519Command? command;
|
||||
lock (_commandLock)
|
||||
{
|
||||
command = !_motionStarted || _currentCommand is null
|
||||
? null
|
||||
: WithSequence(_currentCommand, response.Sequence);
|
||||
}
|
||||
|
||||
if (command is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||
await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
@@ -0,0 +1,632 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
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 readonly ILogger<FanucStateClient>? _logger;
|
||||
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(), null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options)
|
||||
: this(options, null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数和日志记录器初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options, ILogger<FanucStateClient>? logger)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(options);
|
||||
ValidateOptions(options);
|
||||
_options = options;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <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。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_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 (Exception exception)
|
||||
{
|
||||
_logger?.LogError(exception, "StateClient 连接失败: {Ip}:{Port}", ip, port);
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
}
|
||||
|
||||
_receiveCts.Dispose();
|
||||
_receiveCts = null;
|
||||
throw;
|
||||
}
|
||||
|
||||
_receiveTask = Task.Run(
|
||||
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
|
||||
_receiveCts.Token);
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接并启动接收循环: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开状态通道并停止后台接收循环。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("StateClient Disconnect");
|
||||
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;
|
||||
_logger?.LogInformation("StateClient 接收循环启动: {Ip}:{Port}", ip, port);
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
|
||||
reconnectDelay = _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogInformation("StateClient 接收循环正常取消");
|
||||
return;
|
||||
}
|
||||
catch (TimeoutException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 接收超时");
|
||||
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
|
||||
}
|
||||
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接异常，准备重连");
|
||||
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];
|
||||
long frameCount = 0;
|
||||
FanucStateFrame? lastLoggedFrame = null;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
frameCount++;
|
||||
|
||||
// 仅在状态变化或首次接收时记录 Info，避免高频日志。
|
||||
if (lastLoggedFrame is null
|
||||
|| lastLoggedFrame.CartesianPose[0] != frame.CartesianPose[0]
|
||||
|| !lastLoggedFrame.RawTailWords.SequenceEqual(frame.RawTailWords))
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"StateClient 收到状态帧: pose=[{X:F1}, {Y:F1}, {Z:F1}], tail=[{Tail}]",
|
||||
frame.CartesianPose[0],
|
||||
frame.CartesianPose[1],
|
||||
frame.CartesianPose[2],
|
||||
string.Join(", ", frame.RawTailWords));
|
||||
lastLoggedFrame = frame;
|
||||
}
|
||||
else if (frameCount % 1000 == 0)
|
||||
{
|
||||
_logger?.LogDebug("StateClient 已接收 {Count} 个状态帧", frameCount);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <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
|
||||
{
|
||||
_logger?.LogInformation("StateClient 正在连接 {Ip}:{Port}...", ip, port);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接到 {Ip}:{Port}", ip, port);
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogWarning("StateClient 连接 {Ip}:{Port} 超时", ip, port);
|
||||
tcpClient.Dispose();
|
||||
throw new TimeoutException("状态通道建连超时。");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接 {Ip}:{Port} 失败", ip, port);
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"StateClient 将在 {Delay}ms 后尝试重连 {Ip}:{Port}...",
|
||||
nextDelay.TotalMilliseconds,
|
||||
ip,
|
||||
port);
|
||||
|
||||
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_reconnectAttemptCount++;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation(
|
||||
"StateClient 重连成功: {Ip}:{Port}, 累计重连次数={Count}",
|
||||
ip,
|
||||
port,
|
||||
_reconnectAttemptCount);
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
ex,
|
||||
"StateClient 重连失败: {Ip}:{Port}, 下次等待={NextDelay}ms",
|
||||
ip,
|
||||
port,
|
||||
nextDelay.TotalMilliseconds * 2);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
_logger?.LogWarning("StateClient 接收失败: state={State}, message={Message}", state, 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[] _jointRadians;
|
||||
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));
|
||||
}
|
||||
|
||||
_jointRadians = _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> JointRadians => _jointRadians;
|
||||
|
||||
/// <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 状态帧长度字段与实际长度不一致。");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using Flyshot.Server.Host;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供浏览器调试页所需的运行时配置 API。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 本控制器自身不进入 Swagger 文档（<see cref="ApiExplorerSettingsAttribute.IgnoreApi"/>）。
|
||||
/// 调试页静态资源位于 wwwroot，Swagger 地址由配置 API 下发。
|
||||
/// </remarks>
|
||||
[ApiController]
|
||||
[ApiExplorerSettings(IgnoreApi = true)]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class DebugConsoleController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// Swagger 配置项，用于决定调试页是否对外暴露以及拼接 OpenAPI JSON 地址。
|
||||
/// </summary>
|
||||
private readonly HostSwaggerOptions _swaggerOptions;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化在线调试页控制器。
|
||||
/// </summary>
|
||||
/// <param name="swaggerOptions">来自 <c>Swagger</c> 配置节的标准选项。</param>
|
||||
public DebugConsoleController(IOptions<HostSwaggerOptions> swaggerOptions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(swaggerOptions);
|
||||
_swaggerOptions = swaggerOptions.Value ?? new HostSwaggerOptions();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回静态调试页启动时所需的 Swagger 文档地址。
|
||||
/// </summary>
|
||||
/// <returns>当 Swagger 启用时返回配置；否则返回 404，与 Swagger UI 保持一致的可见性策略。</returns>
|
||||
[HttpGet("/api/debug/config")]
|
||||
public IActionResult GetDebugConfig()
|
||||
{
|
||||
if (!_swaggerOptions.Enabled)
|
||||
{
|
||||
return NotFound();
|
||||
}
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
SwaggerJsonUrl = ResolveSwaggerJsonUrl(_swaggerOptions)
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 <see cref="HostSwaggerOptions.JsonRouteTemplate"/> 与 <see cref="HostSwaggerOptions.DocumentName"/> 解析出 Swagger JSON 实际地址。
|
||||
/// </summary>
|
||||
/// <param name="options">Swagger 配置选项。</param>
|
||||
/// <returns>形如 <c>/swagger/v1/swagger.json</c> 的绝对路径。</returns>
|
||||
private static string ResolveSwaggerJsonUrl(HostSwaggerOptions options)
|
||||
{
|
||||
// Swashbuckle 的 RouteTemplate 不带前导斜杠，这里统一加上保证前端 fetch 走绝对路径。
|
||||
var template = options.JsonRouteTemplate ?? "swagger/{documentName}/swagger.json";
|
||||
var path = template.Replace("{documentName}", options.DocumentName ?? "v1", StringComparison.Ordinal);
|
||||
return path.StartsWith('/') ? path : "/" + path;
|
||||
}
|
||||
}
|
||||
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"
|
||||
});
|
||||
}
|
||||
}
|
||||
1048
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
1048
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
File diff suppressed because it is too large
Load Diff
72
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
72
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
@@ -0,0 +1,72 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供控制器状态快照 API，状态监控页面由 wwwroot 静态资源承载。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class StatusController : ControllerBase
|
||||
{
|
||||
private readonly IControllerClientCompatService _compatService;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态监控控制器。
|
||||
/// </summary>
|
||||
/// <param name="compatService">ControllerClient 兼容层服务。</param>
|
||||
public StatusController(IControllerClientCompatService compatService)
|
||||
{
|
||||
_compatService = compatService ?? throw new ArgumentNullException(nameof(compatService));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供短路由 `/status`，跳转到静态状态页。
|
||||
/// </summary>
|
||||
/// <returns>重定向到 <c>/status.html</c>。</returns>
|
||||
[HttpGet("/status")]
|
||||
public IActionResult StatusPage()
|
||||
{
|
||||
return Redirect("/status.html");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供短路由 `/debug`，跳转到静态调试页。
|
||||
/// </summary>
|
||||
/// <returns>重定向到 <c>/debug.html</c>。</returns>
|
||||
[HttpGet("/debug")]
|
||||
public IActionResult DebugPage()
|
||||
{
|
||||
return Redirect("/debug.html");
|
||||
}
|
||||
|
||||
/// <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,31 @@
|
||||
<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="NLog.Web.AspNetCore" Version="5.3.11" />
|
||||
<PackageReference Include="Swashbuckle.AspNetCore" Version="6.6.2" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<!-- 运行时需要把仓库根目录的 NLog.config 带到 Host 输出目录，确保控制台和文件日志目标生效。 -->
|
||||
<Content Include="..\..\NLog.config" Link="NLog.config" CopyToOutputDirectory="PreserveNewest" />
|
||||
<!-- 运行时配置根目录固定为输出目录 Config，调试和发布都复制仓库内固化配置。 -->
|
||||
<Content Include="..\..\Config\**\*" Link="Config\%(RecursiveDir)%(Filename)%(Extension)" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<!-- 让静态调试页在构建和发布时都物理复制到输出目录，避免运行时只依赖源码树中的 wwwroot。 -->
|
||||
<Content Update="wwwroot\**\*" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
|
||||
</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";
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
using System.Diagnostics;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.Server.Host.Middleware;
|
||||
|
||||
/// <summary>
|
||||
/// HTTP 请求与响应日志中间件。
|
||||
/// 记录每个 HTTP 请求的进入时间、方法、路径、查询串、请求体，
|
||||
/// 以及响应的状态码、耗时和响应体（调试级别）。
|
||||
/// </summary>
|
||||
public sealed class RequestResponseLoggingMiddleware
|
||||
{
|
||||
private readonly RequestDelegate _next;
|
||||
private readonly ILogger<RequestResponseLoggingMiddleware> _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 请求体最大日志长度，超出则截断并附加省略标记。
|
||||
/// </summary>
|
||||
private const int MaxBodyLogLength = 4096;
|
||||
|
||||
/// <summary>
|
||||
/// 请求/响应日志忽略路径前缀列表，用于跳过高频轮询接口的常规日志。
|
||||
/// </summary>
|
||||
private static readonly string[] IgnoredLogPathPrefixes =
|
||||
[
|
||||
"/api/status/snapshot"
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// 初始化请求响应日志中间件。
|
||||
/// </summary>
|
||||
/// <param name="next">下一个中间件委托。</param>
|
||||
/// <param name="logger">日志记录器。</param>
|
||||
public RequestResponseLoggingMiddleware(RequestDelegate next, ILogger<RequestResponseLoggingMiddleware> logger)
|
||||
{
|
||||
_next = next ?? throw new ArgumentNullException(nameof(next));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 处理 HTTP 请求并记录输入输出。
|
||||
/// </summary>
|
||||
/// <param name="context">HTTP 上下文。</param>
|
||||
public async Task InvokeAsync(HttpContext context)
|
||||
{
|
||||
// 高频状态轮询接口只转发请求，不记录请求/响应日志，避免控制台和文件日志被刷屏。
|
||||
if (ShouldSkipRequestResponseLog(context.Request.Path))
|
||||
{
|
||||
await _next(context).ConfigureAwait(false);
|
||||
return;
|
||||
}
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var request = context.Request;
|
||||
var requestId = Activity.Current?.Id ?? context.TraceIdentifier;
|
||||
|
||||
// 记录请求进入信息（Info 级别：方法、路径、查询参数）。
|
||||
_logger.LogInformation(
|
||||
"[HTTP-REQ] [{RequestId}] {Method} {Path}{QueryString} — 客户端 {RemoteIp}",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path,
|
||||
request.QueryString.HasValue ? request.QueryString.Value : string.Empty,
|
||||
context.Connection.RemoteIpAddress);
|
||||
|
||||
// 读取并记录请求体（Debug 级别）。
|
||||
string? requestBody = null;
|
||||
if (request.ContentLength > 0 && request.Body.CanRead)
|
||||
{
|
||||
request.EnableBuffering();
|
||||
requestBody = await ReadBodyAsync(request.Body, context.RequestAborted).ConfigureAwait(false);
|
||||
request.Body.Position = 0;
|
||||
|
||||
if (!string.IsNullOrEmpty(requestBody))
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"[HTTP-REQ-BODY] [{RequestId}] {Body}",
|
||||
requestId,
|
||||
TruncateBody(requestBody));
|
||||
}
|
||||
}
|
||||
|
||||
// 拦截响应流以便读取响应体。
|
||||
var originalResponseBody = context.Response.Body;
|
||||
using var responseBodyStream = new MemoryStream();
|
||||
context.Response.Body = responseBodyStream;
|
||||
|
||||
try
|
||||
{
|
||||
await _next(context).ConfigureAwait(false);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_logger.LogError(
|
||||
exception,
|
||||
"[HTTP-ERR] [{RequestId}] {Method} {Path} 处理过程中发生未捕获异常",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path);
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
stopwatch.Stop();
|
||||
responseBodyStream.Position = 0;
|
||||
var responseBody = await ReadBodyAsync(responseBodyStream, context.RequestAborted).ConfigureAwait(false);
|
||||
responseBodyStream.Position = 0;
|
||||
await responseBodyStream.CopyToAsync(originalResponseBody, context.RequestAborted).ConfigureAwait(false);
|
||||
context.Response.Body = originalResponseBody;
|
||||
|
||||
var statusCode = context.Response.StatusCode;
|
||||
var level = statusCode >= 500 ? LogLevel.Error : statusCode >= 400 ? LogLevel.Warning : LogLevel.Information;
|
||||
|
||||
// 记录响应概要（Info/Warning/Error 级别）。
|
||||
_logger.Log(
|
||||
level,
|
||||
"[HTTP-RES] [{RequestId}] {Method} {Path} => {StatusCode} ({ElapsedMs}ms)",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path,
|
||||
statusCode,
|
||||
stopwatch.ElapsedMilliseconds);
|
||||
|
||||
// 记录响应体（Debug 级别）。
|
||||
if (!string.IsNullOrEmpty(responseBody))
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"[HTTP-RES-BODY] [{RequestId}] {Body}",
|
||||
requestId,
|
||||
TruncateBody(responseBody));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中读取文本内容。
|
||||
/// </summary>
|
||||
private static async Task<string> ReadBodyAsync(Stream stream, CancellationToken cancellationToken)
|
||||
{
|
||||
if (!stream.CanRead)
|
||||
{
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
using var reader = new StreamReader(stream, Encoding.UTF8, leaveOpen: true);
|
||||
var body = await reader.ReadToEndAsync(cancellationToken).ConfigureAwait(false);
|
||||
return body;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 截断过长内容，避免日志膨胀。
|
||||
/// </summary>
|
||||
private static string TruncateBody(string body)
|
||||
{
|
||||
if (body.Length <= MaxBodyLogLength)
|
||||
{
|
||||
return body;
|
||||
}
|
||||
|
||||
return body[..MaxBodyLogLength] + " ... [截断，总长度=" + body.Length + "]";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断当前请求路径是否命中请求/响应日志忽略前缀。
|
||||
/// </summary>
|
||||
private static bool ShouldSkipRequestResponseLog(PathString path)
|
||||
{
|
||||
var pathValue = path.Value;
|
||||
return !string.IsNullOrEmpty(pathValue)
|
||||
&& IgnoredLogPathPrefixes.Any(prefix => pathValue.StartsWith(prefix, StringComparison.OrdinalIgnoreCase));
|
||||
}
|
||||
}
|
||||
@@ -1,13 +1,124 @@
|
||||
var builder = WebApplication.CreateBuilder(args);
|
||||
var app = builder.Build();
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Server.Host;
|
||||
using Flyshot.Server.Host.Middleware;
|
||||
using Microsoft.Extensions.Options;
|
||||
using Microsoft.OpenApi.Models;
|
||||
using NLog.Web;
|
||||
using Swashbuckle.AspNetCore.SwaggerGen;
|
||||
|
||||
app.MapGet("/", () => Results.Redirect("/healthz"));
|
||||
app.MapGet("/healthz", () => Results.Ok(new
|
||||
// NLog: 在 ASP.NET Core 启动前完成配置加载，确保最早期的日志也能被捕获。
|
||||
NLog.LogManager.Setup().LoadConfigurationFromAppSettings();
|
||||
var logger = NLog.LogManager.GetCurrentClassLogger();
|
||||
|
||||
try
|
||||
{
|
||||
status = "ok",
|
||||
service = "flyshot-server-host"
|
||||
}));
|
||||
logger.Info("Flyshot Server Host 启动中...");
|
||||
|
||||
app.Run();
|
||||
var builder = WebApplication.CreateBuilder(args);
|
||||
|
||||
// NLog: 替换默认日志提供者为 NLog，清除其他 Provider 避免重复输出。
|
||||
builder.Logging.ClearProviders();
|
||||
builder.Host.UseNLog();
|
||||
|
||||
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();
|
||||
|
||||
var resolvedSwaggerOptions = app.Services.GetRequiredService<IOptions<HostSwaggerOptions>>().Value;
|
||||
var resolvedCorsOptions = app.Services.GetRequiredService<IOptions<HostCorsOptions>>().Value;
|
||||
|
||||
if (resolvedSwaggerOptions.Enabled)
|
||||
{
|
||||
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.UseStaticFiles();
|
||||
|
||||
// 注册 HTTP 请求/响应日志中间件，记录所有 API 调用的输入输出。
|
||||
app.UseMiddleware<RequestResponseLoggingMiddleware>();
|
||||
|
||||
app.MapControllers();
|
||||
|
||||
logger.Info("Flyshot Server Host 已就绪，开始监听请求。");
|
||||
app.Run();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
logger.Error(exception, "Flyshot Server Host 启动失败。");
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
NLog.LogManager.Shutdown();
|
||||
}
|
||||
|
||||
public partial class Program;
|
||||
|
||||
@@ -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": "*"
|
||||
}
|
||||
|
||||
424
src/Flyshot.Server.Host/wwwroot/assets/debug.css
Normal file
424
src/Flyshot.Server.Host/wwwroot/assets/debug.css
Normal file
@@ -0,0 +1,424 @@
|
||||
:root {
|
||||
color-scheme: light;
|
||||
--bg: #f5f7fb;
|
||||
--surface: #ffffff;
|
||||
--line: #d8dee9;
|
||||
--text: #172033;
|
||||
--muted: #5b667a;
|
||||
--accent: #007c89;
|
||||
--good: #12805c;
|
||||
--warn: #b7791f;
|
||||
--bad: #b42318;
|
||||
--get: #1f6feb;
|
||||
--post: #2da44e;
|
||||
--put: #9a6700;
|
||||
--delete: #cf222e;
|
||||
--code-bg: #f4f6fa;
|
||||
}
|
||||
|
||||
* { 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: 14px;
|
||||
}
|
||||
|
||||
header {
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: var(--surface);
|
||||
position: sticky;
|
||||
top: 0;
|
||||
z-index: 10;
|
||||
}
|
||||
|
||||
.topbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 16px;
|
||||
width: min(1280px, calc(100% - 32px));
|
||||
margin: 0 auto;
|
||||
padding: 18px 0;
|
||||
}
|
||||
|
||||
h1 {
|
||||
margin: 0;
|
||||
font-size: 22px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.actions {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
button {
|
||||
min-height: 34px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: var(--accent);
|
||||
color: #ffffff;
|
||||
font: inherit;
|
||||
cursor: pointer;
|
||||
}
|
||||
|
||||
button.secondary {
|
||||
background: transparent;
|
||||
color: var(--accent);
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
opacity: 0.55;
|
||||
cursor: default;
|
||||
}
|
||||
|
||||
.link-button {
|
||||
display: inline-flex;
|
||||
align-items: center;
|
||||
min-height: 34px;
|
||||
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(1280px, calc(100% - 32px));
|
||||
margin: 22px auto 60px;
|
||||
}
|
||||
|
||||
.meta {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
margin-bottom: 18px;
|
||||
}
|
||||
|
||||
.meta dl {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, minmax(0, 1fr));
|
||||
gap: 0;
|
||||
margin: 0;
|
||||
padding: 12px 16px;
|
||||
}
|
||||
|
||||
.meta dt {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.meta dd {
|
||||
margin: 4px 0 0;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.meta dd.bad { color: var(--bad); }
|
||||
.meta dd.good { color: var(--good); }
|
||||
|
||||
.group {
|
||||
margin-top: 22px;
|
||||
}
|
||||
|
||||
.group h2 {
|
||||
margin: 0 0 10px 4px;
|
||||
font-size: 16px;
|
||||
font-weight: 650;
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
.card {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
margin-bottom: 12px;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.card-head {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 12px;
|
||||
padding: 12px 16px;
|
||||
cursor: pointer;
|
||||
user-select: none;
|
||||
}
|
||||
|
||||
.card-head:hover {
|
||||
background: #fafbfd;
|
||||
}
|
||||
|
||||
.badge {
|
||||
flex: 0 0 auto;
|
||||
min-width: 60px;
|
||||
padding: 3px 10px;
|
||||
border-radius: 999px;
|
||||
text-align: center;
|
||||
color: #ffffff;
|
||||
font-weight: 650;
|
||||
font-size: 12px;
|
||||
letter-spacing: 0.5px;
|
||||
}
|
||||
|
||||
.badge.GET { background: var(--get); }
|
||||
.badge.POST { background: var(--post); }
|
||||
.badge.PUT { background: var(--put); }
|
||||
.badge.DELETE { background: var(--delete); }
|
||||
.badge.OTHER { background: var(--muted); }
|
||||
|
||||
.card-path {
|
||||
flex: 1 1 auto;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 14px;
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.card-summary {
|
||||
flex: 0 1 auto;
|
||||
max-width: 50%;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
overflow: hidden;
|
||||
text-overflow: ellipsis;
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
.card-toggle {
|
||||
flex: 0 0 auto;
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.card-body {
|
||||
padding: 12px 16px 16px;
|
||||
border-top: 1px solid var(--line);
|
||||
}
|
||||
|
||||
.card.collapsed .card-body {
|
||||
display: none;
|
||||
}
|
||||
|
||||
.form-row {
|
||||
display: grid;
|
||||
grid-template-columns: 180px minmax(0, 1fr) 90px;
|
||||
gap: 8px 12px;
|
||||
align-items: center;
|
||||
margin-bottom: 8px;
|
||||
}
|
||||
|
||||
.form-row .name {
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
color: var(--text);
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.form-row .name .required {
|
||||
color: var(--bad);
|
||||
margin-left: 4px;
|
||||
}
|
||||
|
||||
.form-row input[type="text"],
|
||||
.form-row input[type="number"] {
|
||||
width: 100%;
|
||||
min-height: 32px;
|
||||
padding: 4px 10px;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
font: inherit;
|
||||
}
|
||||
|
||||
.form-row .type {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.body-block {
|
||||
margin-top: 6px;
|
||||
}
|
||||
|
||||
.body-label {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 12px;
|
||||
margin-bottom: 6px;
|
||||
}
|
||||
|
||||
.body-label .left {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
textarea.body-editor {
|
||||
width: 100%;
|
||||
min-height: 140px;
|
||||
padding: 10px 12px;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
background: var(--code-bg);
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 13px;
|
||||
resize: vertical;
|
||||
}
|
||||
|
||||
.button-row {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
gap: 8px;
|
||||
margin-top: 12px;
|
||||
}
|
||||
|
||||
.response-block {
|
||||
margin-top: 14px;
|
||||
padding-top: 12px;
|
||||
border-top: 1px dashed var(--line);
|
||||
}
|
||||
|
||||
.response-summary {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
margin-bottom: 8px;
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.status-badge {
|
||||
padding: 2px 8px;
|
||||
border-radius: 4px;
|
||||
color: #ffffff;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.status-badge.s2xx { background: var(--good); }
|
||||
.status-badge.s3xx { background: var(--get); }
|
||||
.status-badge.s4xx { background: var(--warn); }
|
||||
.status-badge.s5xx { background: var(--bad); }
|
||||
.status-badge.error { background: var(--bad); }
|
||||
|
||||
pre.response-body,
|
||||
pre.response-headers {
|
||||
margin: 6px 0 0;
|
||||
padding: 10px 12px;
|
||||
background: var(--code-bg);
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
max-height: 360px;
|
||||
overflow: auto;
|
||||
white-space: pre-wrap;
|
||||
overflow-wrap: anywhere;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 12.5px;
|
||||
}
|
||||
|
||||
pre.response-headers {
|
||||
max-height: 160px;
|
||||
}
|
||||
|
||||
details > summary {
|
||||
cursor: pointer;
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
margin: 6px 0 0;
|
||||
}
|
||||
|
||||
.empty-hint {
|
||||
padding: 12px 0;
|
||||
color: var(--muted);
|
||||
font-style: italic;
|
||||
}
|
||||
|
||||
.history {
|
||||
position: fixed;
|
||||
right: 16px;
|
||||
bottom: 16px;
|
||||
width: 360px;
|
||||
max-width: calc(100vw - 32px);
|
||||
max-height: 50vh;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
box-shadow: 0 8px 24px rgba(23, 32, 51, 0.12);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
z-index: 20;
|
||||
}
|
||||
|
||||
.history h3 {
|
||||
margin: 0;
|
||||
padding: 10px 14px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
font-size: 13px;
|
||||
font-weight: 650;
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.history h3 button {
|
||||
min-height: 24px;
|
||||
padding: 0 8px;
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.history ul {
|
||||
list-style: none;
|
||||
margin: 0;
|
||||
padding: 6px 0;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.history li {
|
||||
padding: 6px 14px;
|
||||
font-size: 12px;
|
||||
border-bottom: 1px solid #edf1f7;
|
||||
display: grid;
|
||||
grid-template-columns: 50px 1fr auto;
|
||||
gap: 8px;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.history li:last-child { border-bottom: 0; }
|
||||
|
||||
.history li .h-method {
|
||||
font-weight: 650;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.history li .h-path {
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
overflow: hidden;
|
||||
text-overflow: ellipsis;
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
@media (max-width: 920px) {
|
||||
.form-row {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.meta dl {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.history {
|
||||
position: static;
|
||||
width: auto;
|
||||
margin-top: 18px;
|
||||
max-height: none;
|
||||
}
|
||||
}
|
||||
670
src/Flyshot.Server.Host/wwwroot/assets/debug.js
Normal file
670
src/Flyshot.Server.Host/wwwroot/assets/debug.js
Normal file
@@ -0,0 +1,670 @@
|
||||
// 静态调试页通过配置 API 获取实际 Swagger JSON 地址，避免硬编码路由前缀。
|
||||
const DEBUG_CONFIG_URL = "/api/debug/config";
|
||||
const STORAGE_PREFIX = "flyshot.debug.";
|
||||
const HISTORY_LIMIT = 10;
|
||||
|
||||
const groupTitleByPrefix = [
|
||||
// 基础与状态分组：探活和状态快照两个固定 API 路径
|
||||
{ match: function (op) { return op.path === "/healthz" || op.path === "/api/status/snapshot"; }, title: "基础与状态" },
|
||||
// 默认兜底：剩余全部走 ControllerClient 兼容分组
|
||||
{ match: function () { return true; }, title: "ControllerClient 兼容" }
|
||||
];
|
||||
|
||||
const state = {
|
||||
spec: null,
|
||||
operations: [],
|
||||
history: []
|
||||
};
|
||||
|
||||
/** 简单的 escape：把任意字符串安全嵌入 textContent 之外的位置时使用。 */
|
||||
function escapeHtml(value) {
|
||||
return String(value).replace(/[&<>"']/g, function (ch) {
|
||||
return { "&": "&", "<": "<", ">": ">", "\"": """, "'": "'" }[ch];
|
||||
});
|
||||
}
|
||||
|
||||
/** 解析 OpenAPI 中的 $ref 引用，仅支持本地 components.schemas 形式。 */
|
||||
function resolveRef(ref) {
|
||||
if (!ref || !state.spec) return null;
|
||||
const parts = ref.replace(/^#\//, "").split("/");
|
||||
let cursor = state.spec;
|
||||
for (const part of parts) {
|
||||
if (cursor && Object.prototype.hasOwnProperty.call(cursor, part)) {
|
||||
cursor = cursor[part];
|
||||
} else {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
return cursor;
|
||||
}
|
||||
|
||||
/** 根据 schema 生成默认 JSON 模板，用于自动填充请求体编辑器。 */
|
||||
function buildSampleFromSchema(schema, depth) {
|
||||
depth = depth || 0;
|
||||
// 防御递归：复杂自引用 schema 在 4 层后停下，避免栈爆。
|
||||
if (!schema || depth > 4) return null;
|
||||
|
||||
if (schema.$ref) {
|
||||
const resolved = resolveRef(schema.$ref);
|
||||
return resolved ? buildSampleFromSchema(resolved, depth + 1) : null;
|
||||
}
|
||||
|
||||
// 部分 schema 只标 oneOf/anyOf/allOf，挑第一个分支即可，调试场景够用。
|
||||
if (Array.isArray(schema.oneOf) && schema.oneOf.length > 0) return buildSampleFromSchema(schema.oneOf[0], depth + 1);
|
||||
if (Array.isArray(schema.anyOf) && schema.anyOf.length > 0) return buildSampleFromSchema(schema.anyOf[0], depth + 1);
|
||||
if (Array.isArray(schema.allOf) && schema.allOf.length > 0) {
|
||||
const merged = {};
|
||||
schema.allOf.forEach(function (sub) {
|
||||
const value = buildSampleFromSchema(sub, depth + 1);
|
||||
if (value && typeof value === "object" && !Array.isArray(value)) Object.assign(merged, value);
|
||||
});
|
||||
return merged;
|
||||
}
|
||||
|
||||
const type = schema.type || (schema.properties ? "object" : "string");
|
||||
switch (type) {
|
||||
case "object": {
|
||||
const result = {};
|
||||
const props = schema.properties || {};
|
||||
Object.keys(props).forEach(function (key) {
|
||||
result[key] = buildSampleFromSchema(props[key], depth + 1);
|
||||
});
|
||||
return result;
|
||||
}
|
||||
case "array":
|
||||
return [];
|
||||
case "integer":
|
||||
case "number":
|
||||
return 0;
|
||||
case "boolean":
|
||||
return false;
|
||||
case "string":
|
||||
default:
|
||||
if (schema.enum && schema.enum.length > 0) return schema.enum[0];
|
||||
return "";
|
||||
}
|
||||
}
|
||||
|
||||
/** 把 schema.type 翻译成 input type 与展示文本。 */
|
||||
function inputKindForType(schema) {
|
||||
if (!schema) return { kind: "text", label: "string" };
|
||||
const type = schema.type || "string";
|
||||
if (type === "boolean") return { kind: "checkbox", label: "boolean" };
|
||||
if (type === "integer" || type === "number") return { kind: "number", label: type };
|
||||
return { kind: "text", label: type };
|
||||
}
|
||||
|
||||
/** 把 OpenAPI 的 paths 节点展开成扁平的 operation 列表。 */
|
||||
function extractOperations(spec) {
|
||||
const operations = [];
|
||||
const paths = spec.paths || {};
|
||||
Object.keys(paths).forEach(function (path) {
|
||||
const pathItem = paths[path] || {};
|
||||
["get", "post", "put", "delete", "patch", "options", "head"].forEach(function (method) {
|
||||
const op = pathItem[method];
|
||||
if (!op) return;
|
||||
const parameters = (op.parameters || []).filter(function (p) { return p.in === "query" || p.in === "path"; });
|
||||
let bodySchema = null;
|
||||
if (op.requestBody && op.requestBody.content) {
|
||||
const json = op.requestBody.content["application/json"];
|
||||
if (json && json.schema) bodySchema = json.schema;
|
||||
}
|
||||
operations.push({
|
||||
method: method.toUpperCase(),
|
||||
path: path,
|
||||
summary: op.summary || "",
|
||||
description: op.description || "",
|
||||
tags: op.tags || [],
|
||||
parameters: parameters,
|
||||
bodySchema: bodySchema
|
||||
});
|
||||
});
|
||||
});
|
||||
return operations;
|
||||
}
|
||||
|
||||
/** 选择分组：优先用第一条匹配的 groupTitleByPrefix 规则，OpenAPI tag 留作兜底。 */
|
||||
function pickGroup(op) {
|
||||
for (const rule of groupTitleByPrefix) {
|
||||
if (rule.match(op)) return rule.title;
|
||||
}
|
||||
if (op.tags && op.tags.length > 0) return op.tags[0];
|
||||
return "其它";
|
||||
}
|
||||
|
||||
/** localStorage key 必须避免冲突，使用 method:path 复合键。 */
|
||||
function storageKey(op) {
|
||||
return STORAGE_PREFIX + op.method + ":" + op.path;
|
||||
}
|
||||
|
||||
/** 读取本端点最近一次输入；解析失败则当作空。 */
|
||||
function loadInputs(op) {
|
||||
try {
|
||||
const raw = window.localStorage.getItem(storageKey(op));
|
||||
return raw ? JSON.parse(raw) : null;
|
||||
} catch (e) {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/** 保存本端点最近一次输入；写入失败时静默忽略，避免影响调试体验。 */
|
||||
function saveInputs(op, payload) {
|
||||
try {
|
||||
window.localStorage.setItem(storageKey(op), JSON.stringify(payload));
|
||||
} catch (e) {
|
||||
// localStorage 可能被禁用或满载，忽略写入失败。
|
||||
}
|
||||
}
|
||||
|
||||
/** 拼接最终请求 URL（含 query 串与 path 参数替换）。 */
|
||||
function buildRequestUrl(op, paramValues) {
|
||||
let path = op.path;
|
||||
const queryPairs = [];
|
||||
op.parameters.forEach(function (param) {
|
||||
const raw = paramValues[param.name];
|
||||
if (raw === undefined || raw === null || raw === "") return;
|
||||
if (param.in === "path") {
|
||||
path = path.replace("{" + param.name + "}", encodeURIComponent(raw));
|
||||
} else if (param.in === "query") {
|
||||
queryPairs.push(encodeURIComponent(param.name) + "=" + encodeURIComponent(raw));
|
||||
}
|
||||
});
|
||||
return path + (queryPairs.length > 0 ? "?" + queryPairs.join("&") : "");
|
||||
}
|
||||
|
||||
/** 生成与浏览器请求等价的 curl 命令，便于复制到终端复现。 */
|
||||
function buildCurlCommand(op, requestUrl, body) {
|
||||
const parts = ["curl", "-X", op.method, JSON.stringify(window.location.origin + requestUrl)];
|
||||
if (body !== null && body !== undefined && body !== "") {
|
||||
parts.push("-H", "\"Content-Type: application/json\"");
|
||||
parts.push("--data-raw", JSON.stringify(body));
|
||||
}
|
||||
return parts.join(" ");
|
||||
}
|
||||
|
||||
/** 渲染参数输入表单，返回收集函数。 */
|
||||
function renderParameterRows(container, op, savedValues) {
|
||||
if (op.parameters.length === 0) return function () { return {}; };
|
||||
|
||||
const inputs = {};
|
||||
op.parameters.forEach(function (param) {
|
||||
const row = document.createElement("div");
|
||||
row.className = "form-row";
|
||||
|
||||
const nameNode = document.createElement("div");
|
||||
nameNode.className = "name";
|
||||
nameNode.textContent = param.name + " (" + param.in + ")";
|
||||
if (param.required) {
|
||||
const requiredMark = document.createElement("span");
|
||||
requiredMark.className = "required";
|
||||
requiredMark.textContent = "*";
|
||||
nameNode.appendChild(requiredMark);
|
||||
}
|
||||
row.appendChild(nameNode);
|
||||
|
||||
const kind = inputKindForType(param.schema);
|
||||
const inputNode = document.createElement("input");
|
||||
inputNode.type = kind.kind;
|
||||
if (kind.kind === "checkbox") {
|
||||
inputNode.checked = savedValues && Object.prototype.hasOwnProperty.call(savedValues, param.name)
|
||||
? Boolean(savedValues[param.name])
|
||||
: Boolean(param.schema && param.schema.default);
|
||||
} else {
|
||||
let initial = "";
|
||||
if (savedValues && Object.prototype.hasOwnProperty.call(savedValues, param.name)) {
|
||||
initial = String(savedValues[param.name]);
|
||||
} else if (param.schema && param.schema.default !== undefined) {
|
||||
initial = String(param.schema.default);
|
||||
}
|
||||
inputNode.value = initial;
|
||||
if (kind.kind === "number") inputNode.step = "any";
|
||||
}
|
||||
row.appendChild(inputNode);
|
||||
|
||||
const typeNode = document.createElement("div");
|
||||
typeNode.className = "type";
|
||||
typeNode.textContent = kind.label;
|
||||
row.appendChild(typeNode);
|
||||
|
||||
container.appendChild(row);
|
||||
inputs[param.name] = { node: inputNode, kind: kind.kind, schema: param.schema };
|
||||
});
|
||||
|
||||
return function collect() {
|
||||
const collected = {};
|
||||
Object.keys(inputs).forEach(function (key) {
|
||||
const item = inputs[key];
|
||||
if (item.kind === "checkbox") {
|
||||
collected[key] = item.node.checked;
|
||||
} else {
|
||||
const raw = item.node.value;
|
||||
if (raw === "") {
|
||||
collected[key] = "";
|
||||
} else if (item.kind === "number") {
|
||||
const num = Number(raw);
|
||||
collected[key] = Number.isNaN(num) ? raw : num;
|
||||
} else {
|
||||
collected[key] = raw;
|
||||
}
|
||||
}
|
||||
});
|
||||
return collected;
|
||||
};
|
||||
}
|
||||
|
||||
/** 渲染请求体编辑器，返回收集函数。 */
|
||||
function renderBodyEditor(container, op, savedBody) {
|
||||
if (!op.bodySchema) return function () { return null; };
|
||||
|
||||
const block = document.createElement("div");
|
||||
block.className = "body-block";
|
||||
|
||||
const labelRow = document.createElement("div");
|
||||
labelRow.className = "body-label";
|
||||
const left = document.createElement("div");
|
||||
left.className = "left";
|
||||
left.textContent = "请求体 (application/json)";
|
||||
labelRow.appendChild(left);
|
||||
|
||||
const formatBtn = document.createElement("button");
|
||||
formatBtn.type = "button";
|
||||
formatBtn.className = "secondary";
|
||||
formatBtn.textContent = "格式化 JSON";
|
||||
labelRow.appendChild(formatBtn);
|
||||
|
||||
block.appendChild(labelRow);
|
||||
|
||||
const textarea = document.createElement("textarea");
|
||||
textarea.className = "body-editor";
|
||||
textarea.spellcheck = false;
|
||||
let initialText;
|
||||
if (savedBody !== undefined && savedBody !== null) {
|
||||
initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2);
|
||||
} else {
|
||||
const sample = buildSampleFromSchema(op.bodySchema, 0);
|
||||
initialText = sample === null ? "" : JSON.stringify(sample, null, 2);
|
||||
}
|
||||
textarea.value = initialText;
|
||||
block.appendChild(textarea);
|
||||
|
||||
formatBtn.addEventListener("click", function () {
|
||||
try {
|
||||
const parsed = JSON.parse(textarea.value || "null");
|
||||
textarea.value = parsed === null ? "" : JSON.stringify(parsed, null, 2);
|
||||
} catch (e) {
|
||||
window.alert("JSON 解析失败: " + e.message);
|
||||
}
|
||||
});
|
||||
|
||||
container.appendChild(block);
|
||||
|
||||
return function collect() {
|
||||
return textarea.value;
|
||||
};
|
||||
}
|
||||
|
||||
/** 把 HTTP 状态码翻译成颜色徽标 class。 */
|
||||
function statusBadgeClass(status) {
|
||||
if (status >= 200 && status < 300) return "s2xx";
|
||||
if (status >= 300 && status < 400) return "s3xx";
|
||||
if (status >= 400 && status < 500) return "s4xx";
|
||||
if (status >= 500) return "s5xx";
|
||||
return "error";
|
||||
}
|
||||
|
||||
/** 把响应头展开成可读字符串。 */
|
||||
function formatHeaders(headers) {
|
||||
const lines = [];
|
||||
headers.forEach(function (value, key) { lines.push(key + ": " + value); });
|
||||
return lines.join("\n");
|
||||
}
|
||||
|
||||
/** 在历史面板顶部追加一条记录，超过上限则丢弃尾部。 */
|
||||
function pushHistory(entry) {
|
||||
state.history.unshift(entry);
|
||||
if (state.history.length > HISTORY_LIMIT) state.history.length = HISTORY_LIMIT;
|
||||
renderHistory();
|
||||
}
|
||||
|
||||
function renderHistory() {
|
||||
const list = document.getElementById("history-list");
|
||||
list.innerHTML = "";
|
||||
if (state.history.length === 0) {
|
||||
const empty = document.createElement("li");
|
||||
empty.textContent = "暂无调用记录";
|
||||
empty.style.color = "var(--muted)";
|
||||
empty.style.gridTemplateColumns = "1fr";
|
||||
list.appendChild(empty);
|
||||
return;
|
||||
}
|
||||
state.history.forEach(function (entry) {
|
||||
const li = document.createElement("li");
|
||||
const method = document.createElement("span");
|
||||
method.className = "h-method";
|
||||
method.textContent = entry.method;
|
||||
method.style.color = entry.method === "GET" ? "var(--get)" : entry.method === "POST" ? "var(--post)" : "var(--muted)";
|
||||
const path = document.createElement("span");
|
||||
path.className = "h-path";
|
||||
path.title = entry.url;
|
||||
path.textContent = entry.url;
|
||||
const meta = document.createElement("span");
|
||||
meta.style.color = "var(--muted)";
|
||||
meta.textContent = (entry.status || "ERR") + " · " + entry.elapsedMs + "ms";
|
||||
li.appendChild(method);
|
||||
li.appendChild(path);
|
||||
li.appendChild(meta);
|
||||
list.appendChild(li);
|
||||
});
|
||||
}
|
||||
|
||||
/** 渲染单个端点的卡片。 */
|
||||
function renderOperationCard(op) {
|
||||
const card = document.createElement("section");
|
||||
card.className = "card collapsed";
|
||||
|
||||
const head = document.createElement("div");
|
||||
head.className = "card-head";
|
||||
|
||||
const badge = document.createElement("span");
|
||||
badge.className = "badge " + (["GET", "POST", "PUT", "DELETE"].indexOf(op.method) >= 0 ? op.method : "OTHER");
|
||||
badge.textContent = op.method;
|
||||
head.appendChild(badge);
|
||||
|
||||
const path = document.createElement("span");
|
||||
path.className = "card-path";
|
||||
path.textContent = op.path;
|
||||
head.appendChild(path);
|
||||
|
||||
const summary = document.createElement("span");
|
||||
summary.className = "card-summary";
|
||||
summary.textContent = op.summary;
|
||||
summary.title = op.summary;
|
||||
head.appendChild(summary);
|
||||
|
||||
const toggle = document.createElement("span");
|
||||
toggle.className = "card-toggle";
|
||||
toggle.textContent = "展开 ▾";
|
||||
head.appendChild(toggle);
|
||||
|
||||
head.addEventListener("click", function () {
|
||||
const collapsed = card.classList.toggle("collapsed");
|
||||
toggle.textContent = collapsed ? "展开 ▾" : "收起 ▴";
|
||||
});
|
||||
card.appendChild(head);
|
||||
|
||||
const body = document.createElement("div");
|
||||
body.className = "card-body";
|
||||
|
||||
// 描述（来自 XML summary）独立成一段
|
||||
if (op.summary) {
|
||||
const desc = document.createElement("div");
|
||||
desc.style.color = "var(--muted)";
|
||||
desc.style.marginBottom = "10px";
|
||||
desc.style.fontSize = "13px";
|
||||
desc.textContent = op.summary;
|
||||
body.appendChild(desc);
|
||||
}
|
||||
|
||||
const saved = loadInputs(op) || {};
|
||||
|
||||
// 参数区
|
||||
let collectParams = function () { return {}; };
|
||||
if (op.parameters.length > 0) {
|
||||
const paramsContainer = document.createElement("div");
|
||||
paramsContainer.className = "params";
|
||||
body.appendChild(paramsContainer);
|
||||
collectParams = renderParameterRows(paramsContainer, op, saved.params);
|
||||
}
|
||||
|
||||
// 请求体区
|
||||
const collectBody = renderBodyEditor(body, op, saved.body);
|
||||
|
||||
// 操作按钮
|
||||
const buttonRow = document.createElement("div");
|
||||
buttonRow.className = "button-row";
|
||||
|
||||
const sendBtn = document.createElement("button");
|
||||
sendBtn.type = "button";
|
||||
sendBtn.textContent = "发送";
|
||||
|
||||
const resetBtn = document.createElement("button");
|
||||
resetBtn.type = "button";
|
||||
resetBtn.className = "secondary";
|
||||
resetBtn.textContent = "重置";
|
||||
|
||||
const curlBtn = document.createElement("button");
|
||||
curlBtn.type = "button";
|
||||
curlBtn.className = "secondary";
|
||||
curlBtn.textContent = "复制 curl";
|
||||
|
||||
buttonRow.appendChild(sendBtn);
|
||||
buttonRow.appendChild(resetBtn);
|
||||
buttonRow.appendChild(curlBtn);
|
||||
body.appendChild(buttonRow);
|
||||
|
||||
// 响应面板
|
||||
const responseBlock = document.createElement("div");
|
||||
responseBlock.className = "response-block";
|
||||
responseBlock.style.display = "none";
|
||||
body.appendChild(responseBlock);
|
||||
|
||||
function renderResponse(payload) {
|
||||
responseBlock.style.display = "block";
|
||||
responseBlock.innerHTML = "";
|
||||
|
||||
const summaryRow = document.createElement("div");
|
||||
summaryRow.className = "response-summary";
|
||||
|
||||
const statusBadge = document.createElement("span");
|
||||
statusBadge.className = "status-badge " + statusBadgeClass(payload.status || 0);
|
||||
statusBadge.textContent = payload.status ? payload.status + " " + (payload.statusText || "") : "请求失败";
|
||||
summaryRow.appendChild(statusBadge);
|
||||
|
||||
const elapsed = document.createElement("span");
|
||||
elapsed.style.color = "var(--muted)";
|
||||
elapsed.textContent = payload.elapsedMs + " ms · " + payload.url;
|
||||
summaryRow.appendChild(elapsed);
|
||||
|
||||
responseBlock.appendChild(summaryRow);
|
||||
|
||||
if (payload.error) {
|
||||
const pre = document.createElement("pre");
|
||||
pre.className = "response-body";
|
||||
pre.textContent = payload.error;
|
||||
responseBlock.appendChild(pre);
|
||||
return;
|
||||
}
|
||||
|
||||
const headersDetails = document.createElement("details");
|
||||
const headersSummary = document.createElement("summary");
|
||||
headersSummary.textContent = "响应头";
|
||||
headersDetails.appendChild(headersSummary);
|
||||
const headersPre = document.createElement("pre");
|
||||
headersPre.className = "response-headers";
|
||||
headersPre.textContent = payload.headers;
|
||||
headersDetails.appendChild(headersPre);
|
||||
responseBlock.appendChild(headersDetails);
|
||||
|
||||
const bodyPre = document.createElement("pre");
|
||||
bodyPre.className = "response-body";
|
||||
bodyPre.textContent = payload.bodyText;
|
||||
responseBlock.appendChild(bodyPre);
|
||||
}
|
||||
|
||||
sendBtn.addEventListener("click", async function () {
|
||||
sendBtn.disabled = true;
|
||||
const params = collectParams();
|
||||
const rawBody = collectBody();
|
||||
saveInputs(op, { params: params, body: rawBody });
|
||||
|
||||
const requestUrl = buildRequestUrl(op, params);
|
||||
const init = { method: op.method, headers: {} };
|
||||
|
||||
// 仅 POST/PUT/PATCH/DELETE 才认为可能携带 body；对没有 bodySchema 的方法直接跳过。
|
||||
const methodAllowsBody = ["POST", "PUT", "PATCH", "DELETE"].indexOf(op.method) >= 0;
|
||||
if (methodAllowsBody && op.bodySchema && rawBody !== null && rawBody !== undefined && rawBody !== "") {
|
||||
init.headers["Content-Type"] = "application/json";
|
||||
init.body = rawBody;
|
||||
}
|
||||
|
||||
const startedAt = performance.now();
|
||||
try {
|
||||
const response = await fetch(requestUrl, init);
|
||||
const elapsedMs = Math.round(performance.now() - startedAt);
|
||||
const text = await response.text();
|
||||
const contentType = response.headers.get("content-type") || "";
|
||||
let bodyText = text;
|
||||
if (contentType.indexOf("application/json") >= 0) {
|
||||
try {
|
||||
bodyText = JSON.stringify(JSON.parse(text), null, 2);
|
||||
} catch (e) {
|
||||
bodyText = text;
|
||||
}
|
||||
}
|
||||
renderResponse({
|
||||
status: response.status,
|
||||
statusText: response.statusText,
|
||||
headers: formatHeaders(response.headers),
|
||||
bodyText: bodyText,
|
||||
url: requestUrl,
|
||||
elapsedMs: elapsedMs
|
||||
});
|
||||
pushHistory({ method: op.method, url: requestUrl, status: response.status, elapsedMs: elapsedMs });
|
||||
} catch (err) {
|
||||
const elapsedMs = Math.round(performance.now() - startedAt);
|
||||
renderResponse({
|
||||
error: String(err && err.message ? err.message : err),
|
||||
url: requestUrl,
|
||||
elapsedMs: elapsedMs
|
||||
});
|
||||
pushHistory({ method: op.method, url: requestUrl, status: 0, elapsedMs: elapsedMs });
|
||||
} finally {
|
||||
sendBtn.disabled = false;
|
||||
}
|
||||
});
|
||||
|
||||
resetBtn.addEventListener("click", function () {
|
||||
try { window.localStorage.removeItem(storageKey(op)); } catch (e) { /* 忽略 */ }
|
||||
// 直接重新渲染当前卡片：替换原 DOM 节点。
|
||||
const refreshed = renderOperationCard(op);
|
||||
refreshed.classList.remove("collapsed");
|
||||
refreshed.querySelector(".card-toggle").textContent = "收起 ▴";
|
||||
card.parentNode.replaceChild(refreshed, card);
|
||||
});
|
||||
|
||||
curlBtn.addEventListener("click", function () {
|
||||
const params = collectParams();
|
||||
const rawBody = collectBody();
|
||||
const requestUrl = buildRequestUrl(op, params);
|
||||
const methodAllowsBody = ["POST", "PUT", "PATCH", "DELETE"].indexOf(op.method) >= 0;
|
||||
const bodyForCurl = methodAllowsBody && op.bodySchema ? rawBody : null;
|
||||
const command = buildCurlCommand(op, requestUrl, bodyForCurl);
|
||||
if (navigator.clipboard && navigator.clipboard.writeText) {
|
||||
navigator.clipboard.writeText(command).then(function () {
|
||||
curlBtn.textContent = "已复制 ✓";
|
||||
window.setTimeout(function () { curlBtn.textContent = "复制 curl"; }, 1500);
|
||||
}).catch(function () {
|
||||
window.prompt("复制失败，手动复制：", command);
|
||||
});
|
||||
} else {
|
||||
window.prompt("复制失败，手动复制：", command);
|
||||
}
|
||||
});
|
||||
|
||||
card.appendChild(body);
|
||||
return card;
|
||||
}
|
||||
|
||||
/** 把 operation 列表按分组渲染到主区域。 */
|
||||
function renderGroups(operations) {
|
||||
const root = document.getElementById("debug-console-app");
|
||||
root.innerHTML = "";
|
||||
if (operations.length === 0) {
|
||||
const empty = document.createElement("div");
|
||||
empty.className = "empty-hint";
|
||||
empty.textContent = "OpenAPI 文档中没有任何端点。";
|
||||
root.appendChild(empty);
|
||||
return;
|
||||
}
|
||||
|
||||
const grouped = new Map();
|
||||
operations.forEach(function (op) {
|
||||
const groupTitle = pickGroup(op);
|
||||
if (!grouped.has(groupTitle)) grouped.set(groupTitle, []);
|
||||
grouped.get(groupTitle).push(op);
|
||||
});
|
||||
|
||||
// 固定输出顺序：基础与状态在前，ControllerClient 兼容在后，其余按字典序。
|
||||
const orderedTitles = [];
|
||||
["基础与状态", "ControllerClient 兼容"].forEach(function (title) {
|
||||
if (grouped.has(title)) orderedTitles.push(title);
|
||||
});
|
||||
Array.from(grouped.keys()).sort().forEach(function (title) {
|
||||
if (orderedTitles.indexOf(title) < 0) orderedTitles.push(title);
|
||||
});
|
||||
|
||||
orderedTitles.forEach(function (title) {
|
||||
const ops = grouped.get(title);
|
||||
ops.sort(function (a, b) {
|
||||
if (a.path === b.path) return a.method.localeCompare(b.method);
|
||||
return a.path.localeCompare(b.path);
|
||||
});
|
||||
|
||||
const section = document.createElement("section");
|
||||
section.className = "group";
|
||||
const heading = document.createElement("h2");
|
||||
heading.textContent = title + " (" + ops.length + ")";
|
||||
section.appendChild(heading);
|
||||
ops.forEach(function (op) { section.appendChild(renderOperationCard(op)); });
|
||||
root.appendChild(section);
|
||||
});
|
||||
}
|
||||
|
||||
/** 加载 OpenAPI 文档并渲染。 */
|
||||
async function loadSpecAndRender() {
|
||||
const metaSpec = document.getElementById("meta-spec-url");
|
||||
const metaCount = document.getElementById("meta-operation-count");
|
||||
const metaStatus = document.getElementById("meta-status");
|
||||
|
||||
metaSpec.textContent = "正在读取调试配置...";
|
||||
metaStatus.textContent = "正在拉取 OpenAPI 文档...";
|
||||
metaStatus.className = "";
|
||||
|
||||
try {
|
||||
const configResponse = await fetch(DEBUG_CONFIG_URL, { cache: "no-store" });
|
||||
if (!configResponse.ok) throw new Error("调试配置 HTTP " + configResponse.status + " " + configResponse.statusText);
|
||||
const config = await configResponse.json();
|
||||
const swaggerJsonUrl = config.swaggerJsonUrl;
|
||||
if (!swaggerJsonUrl) throw new Error("调试配置缺少 swaggerJsonUrl");
|
||||
metaSpec.textContent = swaggerJsonUrl;
|
||||
|
||||
const response = await fetch(swaggerJsonUrl, { cache: "no-store" });
|
||||
if (!response.ok) throw new Error("HTTP " + response.status + " " + response.statusText);
|
||||
const spec = await response.json();
|
||||
state.spec = spec;
|
||||
state.operations = extractOperations(spec);
|
||||
metaCount.textContent = state.operations.length;
|
||||
metaStatus.textContent = "已加载";
|
||||
metaStatus.className = "good";
|
||||
renderGroups(state.operations);
|
||||
} catch (err) {
|
||||
metaStatus.textContent = "加载失败: " + (err && err.message ? err.message : err);
|
||||
metaStatus.className = "bad";
|
||||
metaCount.textContent = "0";
|
||||
const root = document.getElementById("debug-console-app");
|
||||
root.innerHTML = "";
|
||||
const errBlock = document.createElement("div");
|
||||
errBlock.className = "empty-hint";
|
||||
errBlock.textContent = "无法加载 OpenAPI 文档，请确认 Swagger:Enabled = true 且 " + DEBUG_CONFIG_URL + " 可访问。";
|
||||
root.appendChild(errBlock);
|
||||
}
|
||||
}
|
||||
|
||||
document.getElementById("reload-spec").addEventListener("click", loadSpecAndRender);
|
||||
document.getElementById("history-clear").addEventListener("click", function () {
|
||||
state.history.length = 0;
|
||||
renderHistory();
|
||||
});
|
||||
|
||||
renderHistory();
|
||||
loadSpecAndRender();
|
||||
214
src/Flyshot.Server.Host/wwwroot/assets/status.css
Normal file
214
src/Flyshot.Server.Host/wwwroot/assets/status.css
Normal file
@@ -0,0 +1,214 @@
|
||||
: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;
|
||||
}
|
||||
}
|
||||
92
src/Flyshot.Server.Host/wwwroot/assets/status.js
Normal file
92
src/Flyshot.Server.Host/wwwroot/assets/status.js
Normal file
@@ -0,0 +1,92 @@
|
||||
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"),
|
||||
j519Status: document.getElementById("j519-status"),
|
||||
j519Sequence: document.getElementById("j519-sequence"),
|
||||
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 formatNullableBool(value) {
|
||||
if (value === true) {
|
||||
return "是";
|
||||
}
|
||||
|
||||
if (value === false) {
|
||||
return "否";
|
||||
}
|
||||
|
||||
return "--";
|
||||
}
|
||||
|
||||
function formatJ519Status(snapshot) {
|
||||
if (snapshot.j519Status === null || snapshot.j519Status === undefined) {
|
||||
return "--";
|
||||
}
|
||||
|
||||
const status = Number(snapshot.j519Status).toString(16).padStart(2, "0").toUpperCase();
|
||||
return `0x${status} accept=${formatNullableBool(snapshot.j519AcceptsCommand)} received=${formatNullableBool(snapshot.j519ReceivedCommand)} sysrdy=${formatNullableBool(snapshot.j519SystemReady)} motion=${formatNullableBool(snapshot.j519RobotInMotion)}`;
|
||||
}
|
||||
|
||||
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.j519Status.textContent = formatJ519Status(snapshot);
|
||||
fields.j519Sequence.textContent = snapshot.j519Sequence ?? "--";
|
||||
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);
|
||||
44
src/Flyshot.Server.Host/wwwroot/debug.html
Normal file
44
src/Flyshot.Server.Host/wwwroot/debug.html
Normal file
@@ -0,0 +1,44 @@
|
||||
<!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>
|
||||
<link rel="stylesheet" href="/assets/debug.css">
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<div class="topbar">
|
||||
<h1>Flyshot Replacement 接口调试</h1>
|
||||
<div class="actions">
|
||||
<a class="link-button" href="/status.html">回到状态页</a>
|
||||
<a class="link-button" href="/swagger" target="_blank" rel="noopener">Swagger UI</a>
|
||||
<button id="reload-spec" type="button">重新加载 OpenAPI</button>
|
||||
</div>
|
||||
</div>
|
||||
</header>
|
||||
<main>
|
||||
<section class="meta">
|
||||
<dl>
|
||||
<dt>OpenAPI 文档</dt>
|
||||
<dd id="meta-spec-url">--</dd>
|
||||
<dt>API 数量</dt>
|
||||
<dd id="meta-operation-count">--</dd>
|
||||
<dt>加载状态</dt>
|
||||
<dd id="meta-status">初始化中...</dd>
|
||||
</dl>
|
||||
</section>
|
||||
<div id="debug-console-app">
|
||||
<div class="empty-hint">正在加载接口列表...</div>
|
||||
</div>
|
||||
</main>
|
||||
<aside class="history" id="history-panel">
|
||||
<h3>
|
||||
<span>调用历史 (本次会话)</span>
|
||||
<button type="button" id="history-clear" class="secondary">清空</button>
|
||||
</h3>
|
||||
<ul id="history-list"></ul>
|
||||
</aside>
|
||||
<script src="/assets/debug.js" defer></script>
|
||||
</body>
|
||||
</html>
|
||||
64
src/Flyshot.Server.Host/wwwroot/status.html
Normal file
64
src/Flyshot.Server.Host/wwwroot/status.html
Normal file
@@ -0,0 +1,64 @@
|
||||
<!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>
|
||||
<link rel="stylesheet" href="/assets/status.css">
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<div class="topbar">
|
||||
<h1>Flyshot Replacement 状态监控</h1>
|
||||
<div class="actions">
|
||||
<a class="link-button" href="/debug.html" 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>J519 状态</dt><dd id="j519-status">--</dd>
|
||||
<dt>J519 序号</dt><dd id="j519-sequence">--</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 src="/assets/status.js" defer></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -24,14 +24,15 @@ public sealed class ConfigCompatibilityTests
|
||||
Assert.Equal(2, loaded.Robot.IoKeepCycles);
|
||||
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
|
||||
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
|
||||
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
||||
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>
|
||||
@@ -71,6 +72,7 @@ public sealed class ConfigCompatibilityTests
|
||||
Assert.Equal(3, loaded.Robot.IoKeepCycles);
|
||||
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
|
||||
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
|
||||
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
|
||||
Assert.Equal([0, 0, 0], program.OffsetValues);
|
||||
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
|
||||
}
|
||||
@@ -80,6 +82,46 @@ public sealed class ConfigCompatibilityTests
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 RobotConfig.json 可以显式配置规划限速倍率，且该倍率独立于运行时 J519 速度倍率。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotConfigLoader_LoadsPlanningSpeedScale()
|
||||
{
|
||||
var tempRoot = CreateTempDirectory();
|
||||
try
|
||||
{
|
||||
var configPath = Path.Combine(tempRoot, "legacy.json");
|
||||
File.WriteAllText(
|
||||
configPath,
|
||||
"""
|
||||
{
|
||||
"robot": {
|
||||
"use_do": true,
|
||||
"io_keep_cycles": 2,
|
||||
"acc_limit": 1.0,
|
||||
"jerk_limit": 1.0,
|
||||
"planning_speed_scale": 0.742277
|
||||
},
|
||||
"flying_shots": {
|
||||
"demo": {
|
||||
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
|
||||
"shot_flags": [false, false, false, false]
|
||||
}
|
||||
}
|
||||
}
|
||||
""");
|
||||
|
||||
var loaded = new RobotConfigLoader().Load(configPath);
|
||||
|
||||
Assert.Equal(0.742277, loaded.Robot.PlanningSpeedScale, precision: 6);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(tempRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
|
||||
/// </summary>
|
||||
@@ -120,10 +162,35 @@ public sealed class ConfigCompatibilityTests
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证路径兼容层既能补旧目录候选，也能按平台策略生成默认用户数据目录。
|
||||
/// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void PathCompatibility_ResolvesLegacyCandidates_AndBuildsUserDataRoots()
|
||||
public void PathCompatibility_ResolvesConfigDirectoryOnly_AndBuildsUserDataRoots()
|
||||
{
|
||||
var tempRoot = CreateTempDirectory();
|
||||
try
|
||||
{
|
||||
var configPath = Path.Combine(tempRoot, "Config", "sample.json");
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(configPath)!);
|
||||
File.WriteAllText(configPath, "{}");
|
||||
|
||||
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
|
||||
|
||||
Assert.Equal(configPath, resolved);
|
||||
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
|
||||
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(tempRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证旧父工作区候选路径存在时也不会被相对配置解析隐式命中。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void PathCompatibility_DoesNotResolveLegacyWorkspaceFallbacks()
|
||||
{
|
||||
var tempRoot = CreateTempDirectory();
|
||||
try
|
||||
@@ -132,11 +199,9 @@ public sealed class ConfigCompatibilityTests
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(legacyConfigPath)!);
|
||||
File.WriteAllText(legacyConfigPath, "{}");
|
||||
|
||||
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
|
||||
var exception = Assert.Throws<FileNotFoundException>(() => PathCompatibility.ResolveConfigPath("sample.json", tempRoot));
|
||||
|
||||
Assert.Equal(legacyConfigPath, resolved);
|
||||
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
|
||||
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
|
||||
Assert.Equal("sample.json", exception.FileName);
|
||||
}
|
||||
finally
|
||||
{
|
||||
@@ -144,6 +209,19 @@ public sealed class ConfigCompatibilityTests
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证默认加载配置时使用当前 replacement 仓库内的 Config/RobotConfig.json。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void RobotConfigLoader_LoadsRepositoryConfigFromReplacementConfigDirectory()
|
||||
{
|
||||
var replacementRoot = GetReplacementRoot();
|
||||
|
||||
var loaded = new RobotConfigLoader().Load("RobotConfig.json");
|
||||
|
||||
Assert.Equal(Path.Combine(replacementRoot, "Config", "RobotConfig.json"), loaded.SourcePath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位当前工作区根目录，便于复用父仓库中的真实样本。
|
||||
/// </summary>
|
||||
@@ -164,6 +242,25 @@ public sealed class ConfigCompatibilityTests
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
|
||||
/// </summary>
|
||||
private static string GetReplacementRoot()
|
||||
{
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建当前测试专用的临时目录，避免不同测试之间相互污染。
|
||||
/// </summary>
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 ControllerClient 兼容层默认围绕运行目录 Config 读写配置和轨迹文件。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatConfigRootTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 验证路径兼容层优先命中运行目录 Config 下的 RobotConfig.json，而不是旧仓库根目录候选。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void PathCompatibility_ResolvesRuntimeConfigBeforeLegacyCandidates()
|
||||
{
|
||||
var runtimeRoot = CreateTempDirectory();
|
||||
try
|
||||
{
|
||||
var configPath = Path.Combine(runtimeRoot, "Config", "RobotConfig.json");
|
||||
var legacyPath = Path.Combine(runtimeRoot, "RobotConfig.json");
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(configPath)!);
|
||||
File.WriteAllText(configPath, "{}");
|
||||
File.WriteAllText(legacyPath, "{}");
|
||||
|
||||
var resolved = PathCompatibility.ResolveConfigPath("RobotConfig.json", runtimeRoot);
|
||||
|
||||
Assert.Equal(configPath, resolved);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(runtimeRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载 .robot 文件。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatRobotCatalog_LoadsModelFromConfigRootModels()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
CopySampleRobotModel(configRoot);
|
||||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||||
var catalog = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader());
|
||||
|
||||
var profile = catalog.LoadProfile("FANUC_LR_Mate_200iD");
|
||||
|
||||
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.robot"), profile.ModelPath);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/RobotConfig.json。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesInRobotConfigJson()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||||
var store = new JsonFlyshotTrajectoryStore(options, new RobotConfigLoader());
|
||||
var settings = new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: [7, 8],
|
||||
ioKeepCycles: 2,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
store.Save("FANUC_LR_Mate_200iD", settings, trajectory);
|
||||
var expectedPath = Path.Combine(configRoot, "RobotConfig.json");
|
||||
|
||||
Assert.True(File.Exists(expectedPath), $"应在运行目录 Config 下创建统一配置文件: {expectedPath}");
|
||||
Assert.False(Directory.Exists(Path.Combine(configRoot, "TrajectoryStore")), "不应再创建独立轨迹存储目录。");
|
||||
var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
|
||||
Assert.NotNull(loadedSettings);
|
||||
Assert.Contains(trajectory.Name, loaded);
|
||||
|
||||
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);
|
||||
|
||||
var afterDelete = store.LoadAll("FANUC_LR_Mate_200iD", out _);
|
||||
Assert.Empty(afterDelete);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建测试专用的运行目录 Config 根，避免污染真实输出目录。
|
||||
/// </summary>
|
||||
private static string CreateTempConfigRoot()
|
||||
{
|
||||
var root = Path.Combine(Path.GetTempPath(), "flyshot-config-root-tests", Guid.NewGuid().ToString("N"), "Config");
|
||||
Directory.CreateDirectory(root);
|
||||
return root;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建测试专用的临时目录。
|
||||
/// </summary>
|
||||
private static string CreateTempDirectory()
|
||||
{
|
||||
var root = Path.Combine(Path.GetTempPath(), "flyshot-config-root-tests", Guid.NewGuid().ToString("N"));
|
||||
Directory.CreateDirectory(root);
|
||||
return root;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复制仓库内已固化的现场机器人模型到临时 Config/Models 目录。
|
||||
/// </summary>
|
||||
private static void CopySampleRobotModel(string configRoot)
|
||||
{
|
||||
var modelDir = Path.Combine(configRoot, "Models");
|
||||
Directory.CreateDirectory(modelDir);
|
||||
File.Copy(
|
||||
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.robot"),
|
||||
Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位 replacement 仓库根目录，供测试读取仓库内固化样本。
|
||||
/// </summary>
|
||||
private static string GetReplacementRoot()
|
||||
{
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
678
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
Normal file
678
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
Normal file
@@ -0,0 +1,678 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using System.Reflection;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
|
||||
/// </summary>
|
||||
public sealed class FanucControllerRuntimeDenseTests
|
||||
{
|
||||
private const double CapturedMvpointVelocityShapeCoefficient = 2.0759961613199973;
|
||||
private const double CapturedMvpointAccelerationShapeCoefficient = 7.986313199999984;
|
||||
private const double CapturedMvpointJerkShapeCoefficient = 36.12609273600853;
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 0.5);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
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, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(5, commands.Count);
|
||||
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
||||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(1.0)]
|
||||
[InlineData(0.7)]
|
||||
[InlineData(0.5)]
|
||||
public void MoveJoint_RealMode_GeneratesTemporaryPtpTrajectoryAndResamplesBySpeedRatio(double speedRatio)
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[]
|
||||
{
|
||||
1.056731,
|
||||
0.011664811,
|
||||
-0.017892333,
|
||||
-0.01516874,
|
||||
0.021492079,
|
||||
0.009567846
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
0.8532358,
|
||||
0.03837953,
|
||||
-0.19235304,
|
||||
0.0071595116,
|
||||
0.109054826,
|
||||
0.040055145
|
||||
};
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = TestRobotFactory.GetConfigRoot()
|
||||
};
|
||||
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
|
||||
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
|
||||
var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
|
||||
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
|
||||
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
|
||||
|
||||
var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(middleAlpha, 0.45, 0.55);
|
||||
|
||||
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
|
||||
Assert.InRange(earlyAlpha, 0.0, 0.02);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_LowerSpeedUsesMoreSamplesWithoutFixedCountContract()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 1.056731, 0.011664811, -0.017892333, -0.01516874, 0.021492079, 0.009567846 };
|
||||
var targetJoints = new[] { 0.8532358, 0.03837953, -0.19235304, 0.0071595116, 0.109054826, 0.040055145 };
|
||||
|
||||
var fullSpeed = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var speed07 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var speed05 = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.5);
|
||||
|
||||
Assert.True(speed07.DenseJointTrajectory!.Count > fullSpeed.DenseJointTrajectory!.Count);
|
||||
Assert.True(speed05.DenseJointTrajectory!.Count > speed07.DenseJointTrajectory!.Count);
|
||||
Assert.InRange(fullSpeed.Duration.TotalSeconds, 0.318, 0.322);
|
||||
Assert.True(speed07.Duration.TotalSeconds >= 0.320);
|
||||
Assert.InRange(speed05.Duration.TotalSeconds, 0.318, 0.322);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var service = CreateCompatService(runtime);
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetPrivateField(runtime, "_jointPositions", startJoints);
|
||||
|
||||
service.MoveJoint(targetJoints);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var currentCommand = j519Client.GetCurrentCommand();
|
||||
Assert.NotNull(currentCommand);
|
||||
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed_ratio=0 时不会启动无法推进轨迹时间的后台发送任务。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsZeroSpeedRatio()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 0.0);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
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);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
Assert.Contains("Speed ratio", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机模式下若 J519 响应明确显示伺服侧未就绪，则拒绝启动稠密轨迹发送。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_RejectsNotReadyJ519Status()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0011);
|
||||
|
||||
var result = CreateDenseResult(
|
||||
[
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
[0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
],
|
||||
durationSeconds: 0.008);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => runtime.ExecuteTrajectory(result, [Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
|
||||
Assert.Contains("J519 status is not ready", exception.Message);
|
||||
Assert.Contains("sysrdy=False", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证控制器快照暴露最近一次 J519 响应中的四个状态位，便于状态页和诊断接口显示。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void GetSnapshot_RealMode_IncludesLatestJ519StatusBits()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
SetLatestJ519Response(j519Client, status: 0b0111);
|
||||
|
||||
var snapshot = runtime.GetSnapshot();
|
||||
|
||||
Assert.Equal((byte)0b0111, snapshot.J519Status);
|
||||
Assert.Equal(10u, snapshot.J519Sequence);
|
||||
Assert.True(snapshot.J519AcceptsCommand);
|
||||
Assert.True(snapshot.J519ReceivedCommand);
|
||||
Assert.True(snapshot.J519SystemReady);
|
||||
Assert.False(snapshot.J519RobotInMotion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍 IO 脉冲按轨迹时间轴嵌入 J519 命令，并在保持周期后用同一 mask 清零。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_EmbedsIoPulseOnTrajectoryTimeline()
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
|
||||
var denseTrajectory = new[]
|
||||
{
|
||||
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||
new[] { 0.024, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||
};
|
||||
|
||||
var result = new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.024),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline:
|
||||
[
|
||||
new TrajectoryDoEvent(
|
||||
waypointIndex: 1,
|
||||
triggerTime: 0.008,
|
||||
offsetCycles: 0,
|
||||
holdCycles: 2,
|
||||
addressGroup: new IoAddressGroup([1, 3]))
|
||||
],
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(4, commands.Count);
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
|
||||
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)0], commands.Select(static command => command.WriteIoValue));
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_MatchesCapturedMvpointAlphaLawAtSpeedOne()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[]
|
||||
{
|
||||
DegreesToRadians(60.546227),
|
||||
DegreesToRadians(0.668344),
|
||||
DegreesToRadians(-1.025155),
|
||||
DegreesToRadians(-0.869105),
|
||||
DegreesToRadians(1.231405),
|
||||
DegreesToRadians(0.548197)
|
||||
};
|
||||
var targetJoints = new[]
|
||||
{
|
||||
DegreesToRadians(48.886810),
|
||||
DegreesToRadians(2.198985),
|
||||
DegreesToRadians(-11.021017),
|
||||
DegreesToRadians(0.410210),
|
||||
DegreesToRadians(6.248381),
|
||||
DegreesToRadians(2.294991)
|
||||
};
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.Equal(41, rows.Count);
|
||||
Assert.InRange(result.Duration.TotalSeconds, 0.318, 0.322);
|
||||
|
||||
var expectedAlpha = new[]
|
||||
{
|
||||
0.000000000000,
|
||||
0.000012196163,
|
||||
0.000106156906,
|
||||
0.000764380061,
|
||||
0.002550804028,
|
||||
0.006029689194,
|
||||
0.011765134027,
|
||||
0.020321400844,
|
||||
0.032262426551,
|
||||
0.048152469303,
|
||||
0.068555498563,
|
||||
0.093895155669,
|
||||
0.124210027377,
|
||||
0.159174512929,
|
||||
0.198230386318,
|
||||
0.240813559900,
|
||||
0.286359937276,
|
||||
0.334305411725,
|
||||
0.384085546646,
|
||||
0.435136609163,
|
||||
0.486894129077,
|
||||
0.538794033110,
|
||||
0.590272360135,
|
||||
0.640764719629,
|
||||
0.689707151220,
|
||||
0.736535405849,
|
||||
0.780685354316,
|
||||
0.821592775628,
|
||||
0.858693734065,
|
||||
0.891423926949,
|
||||
0.919286047395,
|
||||
0.942156722091,
|
||||
0.960255163676,
|
||||
0.974119666692,
|
||||
0.984314536393,
|
||||
0.991403790959,
|
||||
0.995951593494,
|
||||
0.998522142663,
|
||||
0.999679443354,
|
||||
0.999987892657,
|
||||
1.000000000000
|
||||
};
|
||||
|
||||
for (var index = 0; index < rows.Count; index++)
|
||||
{
|
||||
var actualDegrees = rows[index].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
var alpha = ComputeLineAlpha(actualDegrees, startJoints, targetJoints);
|
||||
Assert.Equal(expectedAlpha[index], alpha, precision: 6);
|
||||
}
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_DoesNotShortenBaseDurationWhenSpeedRatioDoesNotDivideWindow()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 0.7);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
|
||||
Assert.True(result.Duration.TotalSeconds >= 0.320, $"Duration was shortened to {result.Duration.TotalSeconds:F6}s.");
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_RejectsUnrepresentableSampleCountForTinySpeedRatio()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { 0.05, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(
|
||||
() => MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1e-12));
|
||||
|
||||
Assert.Contains("sample count", exception.Message, StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void MoveJointTrajectoryGenerator_StretchesLongMoveFromJointLimitsInsteadOfKeepingFortyCycles()
|
||||
{
|
||||
var robot = CreateMoveJointReferenceRobotProfile();
|
||||
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
var targetJoints = new[] { Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 };
|
||||
|
||||
var result = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
|
||||
var rows = result.DenseJointTrajectory!;
|
||||
var expectedVelocityDuration = Math.PI * CapturedMvpointVelocityShapeCoefficient / robot.JointLimits[0].VelocityLimit;
|
||||
var expectedAccelerationDuration = Math.Sqrt(Math.PI * CapturedMvpointAccelerationShapeCoefficient / robot.JointLimits[0].AccelerationLimit);
|
||||
var expectedJerkDuration = Math.Cbrt(Math.PI * CapturedMvpointJerkShapeCoefficient / robot.JointLimits[0].JerkLimit);
|
||||
var expectedMinimumDuration = new[]
|
||||
{
|
||||
0.320,
|
||||
expectedVelocityDuration,
|
||||
expectedAccelerationDuration,
|
||||
expectedJerkDuration
|
||||
}.Max();
|
||||
var expectedCountFromDuration = (int)Math.Floor(result.Duration.TotalSeconds / robot.ServoPeriod.TotalSeconds + 1e-9) + 1;
|
||||
|
||||
Assert.True(rows.Count > 41, $"Expected long MoveJoint to produce more than 41 points, got {rows.Count}.");
|
||||
Assert.True(
|
||||
result.Duration.TotalSeconds >= expectedMinimumDuration,
|
||||
$"Expected duration >= {expectedMinimumDuration:F6}s from joint limits, got {result.Duration.TotalSeconds:F6}s.");
|
||||
Assert.Equal(expectedCountFromDuration, rows.Count);
|
||||
AssertJointDegreesEqual(startJoints, rows[0].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
AssertJointDegreesEqual(targetJoints, rows[^1].Skip(1).Select(RadiansToDegrees).ToArray());
|
||||
}
|
||||
|
||||
private static void ForceRealModeEnabled(FanucControllerRuntime runtime, double speedRatio)
|
||||
{
|
||||
SetPrivateField(runtime, "_activeControllerIsSimulation", false);
|
||||
SetPrivateField(runtime, "_connectedRobotIp", "127.0.0.1");
|
||||
SetPrivateField(runtime, "_isEnabled", true);
|
||||
SetPrivateField(runtime, "_bufferSize", 2);
|
||||
SetPrivateField(runtime, "_speedRatio", speedRatio);
|
||||
}
|
||||
|
||||
private static ControllerClientCompatService CreateCompatService(FanucControllerRuntime runtime)
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = TestRobotFactory.GetConfigRoot()
|
||||
};
|
||||
|
||||
return new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
}
|
||||
|
||||
private static double ComputeLineAlpha(
|
||||
IReadOnlyList<double> actualDegrees,
|
||||
IReadOnlyList<double> startRadians,
|
||||
IReadOnlyList<double> targetRadians)
|
||||
{
|
||||
var numerator = 0.0;
|
||||
var denominator = 0.0;
|
||||
for (var index = 0; index < startRadians.Count; index++)
|
||||
{
|
||||
var startDegrees = RadiansToDegrees(startRadians[index]);
|
||||
var deltaDegrees = RadiansToDegrees(targetRadians[index]) - startDegrees;
|
||||
numerator += (actualDegrees[index] - startDegrees) * deltaDegrees;
|
||||
denominator += deltaDegrees * deltaDegrees;
|
||||
}
|
||||
|
||||
return denominator <= 0.0 ? 0.0 : numerator / denominator;
|
||||
}
|
||||
|
||||
private static void AssertJointDegreesEqual(IReadOnlyList<double> expectedRadians, IReadOnlyList<double> actualDegrees)
|
||||
{
|
||||
Assert.Equal(expectedRadians.Count, actualDegrees.Count);
|
||||
for (var index = 0; index < expectedRadians.Count; index++)
|
||||
{
|
||||
Assert.Equal(RadiansToDegrees(expectedRadians[index]), actualDegrees[index], precision: 4);
|
||||
}
|
||||
}
|
||||
|
||||
private static RobotProfile CreateMoveJointReferenceRobotProfile()
|
||||
{
|
||||
return new RobotProfile(
|
||||
name: "FANUC_LR_Mate_200iD",
|
||||
modelPath: "Models/FANUC_LR_Mate_200iD.robot",
|
||||
degreesOfFreedom: 6,
|
||||
jointLimits:
|
||||
[
|
||||
new JointLimit("J1", 7.85, 32.72, 272.7),
|
||||
new JointLimit("J2", 6.63, 27.63, 230.28),
|
||||
new JointLimit("J3", 9.07, 37.81, 315.12),
|
||||
new JointLimit("J4", 9.59, 39.99, 333.3),
|
||||
new JointLimit("J5", 9.51, 39.63, 330.27),
|
||||
new JointLimit("J6", 17.45, 72.72, 606.01)
|
||||
],
|
||||
jointCouplings: Array.Empty<JointCoupling>(),
|
||||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||||
}
|
||||
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
private static void WaitUntilIdle(FanucControllerRuntime runtime)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
if (!runtime.GetSnapshot().IsInMotion)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Thread.Sleep(1);
|
||||
}
|
||||
|
||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||
}
|
||||
|
||||
private static void SetPrivateField<T>(FanucControllerRuntime runtime, string name, T value)
|
||||
{
|
||||
var field = typeof(FanucControllerRuntime).GetField(name, BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(runtime, value);
|
||||
}
|
||||
|
||||
private static TrajectoryResult CreateDenseResult(IReadOnlyList<IReadOnlyList<double>> denseTrajectory, double durationSeconds)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: "demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
}
|
||||
|
||||
private static void SetLatestJ519Response(FanucJ519Client client, byte status)
|
||||
{
|
||||
var response = new FanucJ519Response(
|
||||
messageType: 0,
|
||||
version: 1,
|
||||
sequence: 10,
|
||||
status: status,
|
||||
readIoType: 2,
|
||||
readIoIndex: 1,
|
||||
readIoMask: 255,
|
||||
readIoValue: 0,
|
||||
timestamp: 1234,
|
||||
pose: new double[6],
|
||||
externalAxes: new double[3],
|
||||
jointDegrees: new double[9],
|
||||
motorCurrents: new double[9]);
|
||||
var field = typeof(FanucJ519Client).GetField("_latestResponse", BindingFlags.Instance | BindingFlags.NonPublic);
|
||||
Assert.NotNull(field);
|
||||
field.SetValue(client, response);
|
||||
}
|
||||
}
|
||||
302
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
302
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
@@ -0,0 +1,302 @@
|
||||
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>
|
||||
/// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
// 接收并丢弃初始化包。
|
||||
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();
|
||||
|
||||
// 机器人尚未回状态包时，上位机不应自行发 64B command packet。
|
||||
await Assert.ThrowsAsync<TimeoutException>(
|
||||
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
|
||||
|
||||
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>
|
||||
/// 验证收到状态包后，下一帧命令使用该状态包的序号。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand()
|
||||
{
|
||||
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, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
|
||||
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||
Assert.Equal(100u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence()
|
||||
{
|
||||
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: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
client.UpdateCommand(command);
|
||||
client.StartMotion();
|
||||
|
||||
var packets = new List<byte[]>();
|
||||
for (uint sequence = 100; sequence < 104; sequence++)
|
||||
{
|
||||
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
packets.Add(result.Buffer);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
var sequences = packets
|
||||
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
|
||||
.ToArray();
|
||||
Assert.Equal([100u, 101u, 102u, 103u], sequences);
|
||||
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence()
|
||||
{
|
||||
using var client = new FanucJ519Client();
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 200);
|
||||
var first = await _server.ReceiveAsync(_cts.Token);
|
||||
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
|
||||
Assert.Equal(200u, firstSequence);
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
byte[] packet;
|
||||
do
|
||||
{
|
||||
packet = (await _server.ReceiveAsync(_cts.Token)).Buffer;
|
||||
}
|
||||
while (packet.Length != FanucJ519Protocol.ControlPacketLength);
|
||||
|
||||
client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
|
||||
client.StartMotion();
|
||||
|
||||
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 300);
|
||||
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
|
||||
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
|
||||
Assert.Equal(300u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)));
|
||||
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.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>
|
||||
/// 验证状态包驱动发送能持续输出命令包。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task StartMotion_MaintainsStatusDrivenCommandStream()
|
||||
{
|
||||
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();
|
||||
|
||||
// 收集 5 个命令包到达时间戳和序号。
|
||||
var timestamps = new List<DateTimeOffset>();
|
||||
var sequences = new List<uint>();
|
||||
for (var i = 0; i < 5; i++)
|
||||
{
|
||||
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: (uint)(500 + i));
|
||||
var result = await _server.ReceiveAsync(_cts.Token);
|
||||
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
|
||||
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
|
||||
timestamps.Add(DateTimeOffset.UtcNow);
|
||||
}
|
||||
|
||||
await client.StopMotionAsync(_cts.Token);
|
||||
|
||||
Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences);
|
||||
|
||||
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
|
||||
var intervals = new List<TimeSpan>();
|
||||
for (var i = 1; i < timestamps.Count; i++)
|
||||
{
|
||||
intervals.Add(timestamps[i] - timestamps[i - 1]);
|
||||
}
|
||||
|
||||
Assert.All(intervals, interval =>
|
||||
{
|
||||
Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
|
||||
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||
/// </summary>
|
||||
private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence)
|
||||
{
|
||||
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), sequence);
|
||||
responsePacket[0x0c] = 15;
|
||||
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
||||
}
|
||||
}
|
||||
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.JointRadians);
|
||||
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.JointRadians.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>
|
||||
|
||||
@@ -35,11 +35,12 @@ public sealed class OfflinePlanTests
|
||||
double speedRatio)
|
||||
{
|
||||
var workspaceRoot = GetWorkspaceRoot();
|
||||
var resolvedConfigPath = Path.Combine(workspaceRoot, configPath);
|
||||
var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}");
|
||||
Directory.CreateDirectory(outputDir);
|
||||
|
||||
// 1. 加载配置和模型。
|
||||
var loadedConfig = new RobotConfigLoader().Load(configPath, repoRoot: workspaceRoot);
|
||||
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
|
||||
var program = loadedConfig.Programs[trajName];
|
||||
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
|
||||
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
|
||||
|
||||
@@ -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>
|
||||
|
||||
899
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
Normal file
899
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
Normal file
@@ -0,0 +1,899 @@
|
||||
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>
|
||||
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
|
||||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0);
|
||||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5);
|
||||
|
||||
Assert.True(
|
||||
halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9,
|
||||
$"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale()
|
||||
{
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||||
var options = new FlyshotExecutionOptions(useCache: true);
|
||||
|
||||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0);
|
||||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5);
|
||||
|
||||
Assert.False(halfSpeed.Result.UsedCache);
|
||||
Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration);
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = configRoot
|
||||
};
|
||||
var runtime = new DelayedCompletionControllerRuntime(
|
||||
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||||
|
||||
service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false));
|
||||
|
||||
Assert.True(runtime.ExecuteCalls.Count >= 2);
|
||||
Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory);
|
||||
Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1);
|
||||
Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
var options = new ControllerClientCompatOptions
|
||||
{
|
||||
ConfigRoot = configRoot
|
||||
};
|
||||
var runtime = new DelayedCompletionControllerRuntime(
|
||||
initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||||
|
||||
service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true));
|
||||
|
||||
Assert.Single(runtime.ExecuteCalls);
|
||||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
File.WriteAllText(
|
||||
Path.Combine(configRoot, "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 { ConfigRoot = configRoot };
|
||||
var runtime = new RecordingControllerRuntime();
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
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(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
new RecordingControllerRuntime(),
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
|
||||
var valid = service.IsFlyshotTrajectoryValid(
|
||||
out var duration,
|
||||
"demo-flyshot",
|
||||
method: "icsp",
|
||||
saveTrajectory: true);
|
||||
|
||||
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
|
||||
Assert.True(valid);
|
||||
Assert.True(duration > TimeSpan.Zero);
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||||
Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json")));
|
||||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
|
||||
/// </summary>
|
||||
private static string CreateTempConfigRoot()
|
||||
{
|
||||
var configRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"), "Config");
|
||||
var modelDir = Path.Combine(configRoot, "Models");
|
||||
Directory.CreateDirectory(modelDir);
|
||||
|
||||
var sourceModel = Path.Combine(
|
||||
TestRobotFactory.GetReplacementRoot(),
|
||||
"Config",
|
||||
"Models",
|
||||
"LR_Mate_200iD_7L.robot");
|
||||
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
|
||||
|
||||
return configRoot;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
|
||||
/// </summary>
|
||||
/// <param name="configRoot">测试运行配置根。</param>
|
||||
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
|
||||
{
|
||||
File.WriteAllText(
|
||||
Path.Combine(configRoot, "RobotConfig.json"),
|
||||
"""
|
||||
{
|
||||
"robot": {
|
||||
"use_do": true,
|
||||
"io_addr": [7, 8],
|
||||
"io_keep_cycles": 2,
|
||||
"acc_limit": 1.0,
|
||||
"jerk_limit": 1.0,
|
||||
"adapt_icsp_try_num": 5
|
||||
},
|
||||
"flying_shots": {
|
||||
"demo-flyshot": {
|
||||
"traj_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]
|
||||
],
|
||||
"shot_flags": [false, true, false, false],
|
||||
"offset_values": [0, 1, 0, 0],
|
||||
"addr": [[], [7, 8], [], []]
|
||||
}
|
||||
}
|
||||
}
|
||||
""");
|
||||
}
|
||||
}
|
||||
|
||||
/// <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
|
||||
{
|
||||
ConfigRoot = GetConfigRoot()
|
||||
};
|
||||
|
||||
return new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
new FanucControllerRuntime(),
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位 replacement 仓库内的运行配置根目录。
|
||||
/// </summary>
|
||||
/// <returns>当前仓库 Config 目录。</returns>
|
||||
public static string GetConfigRoot()
|
||||
{
|
||||
return Path.Combine(GetReplacementRoot(), "Config");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
|
||||
/// </summary>
|
||||
/// <returns>replacement 仓库根目录。</returns>
|
||||
public static string GetReplacementRoot()
|
||||
{
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
|
||||
}
|
||||
|
||||
/// <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>
|
||||
/// 记录 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)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
|
||||
/// </summary>
|
||||
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
|
||||
{
|
||||
private readonly object _lock = new();
|
||||
private readonly TimeSpan _firstMotionCompletionDelay;
|
||||
private double[] _jointPositions;
|
||||
private bool _isEnabled;
|
||||
private bool _isInMotion;
|
||||
private bool _firstMotionCompleted;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化可延迟完成第一段运动的测试运行时。
|
||||
/// </summary>
|
||||
/// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
|
||||
/// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
|
||||
public DelayedCompletionControllerRuntime(
|
||||
IReadOnlyList<double> initialJointPositions,
|
||||
TimeSpan firstMotionCompletionDelay)
|
||||
{
|
||||
_jointPositions = initialJointPositions.ToArray();
|
||||
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取所有 ExecuteTrajectory 调用记录。
|
||||
/// </summary>
|
||||
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
|
||||
/// </summary>
|
||||
public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void SetActiveController(bool sim)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Connect(string robotIp)
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Disconnect()
|
||||
{
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void EnableRobot(int bufferSize)
|
||||
{
|
||||
_isEnabled = true;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void DisableRobot()
|
||||
{
|
||||
_isEnabled = false;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void StopMove()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_isInMotion = false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <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()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _jointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||||
|
||||
/// <inheritdoc />
|
||||
public ControllerStateSnapshot GetSnapshot()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.UtcNow,
|
||||
connectionState: "Connected",
|
||||
isEnabled: _isEnabled,
|
||||
isInMotion: _isInMotion,
|
||||
speedRatio: 1.0,
|
||||
jointPositions: _jointPositions.ToArray(),
|
||||
cartesianPose: Array.Empty<double>(),
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
|
||||
if (ExecuteCalls.Count == 1)
|
||||
{
|
||||
_isInMotion = true;
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
|
||||
lock (_lock)
|
||||
{
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
_isInMotion = false;
|
||||
_firstMotionCompleted = true;
|
||||
}
|
||||
});
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_firstMotionCompleted)
|
||||
{
|
||||
SecondTrajectoryStartedBeforeFirstMotionCompleted = true;
|
||||
}
|
||||
|
||||
_jointPositions = finalJointPositions.ToArray();
|
||||
}
|
||||
}
|
||||
}
|
||||
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs
Normal file
@@ -0,0 +1,526 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Globalization;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
/// <summary>
|
||||
/// 使用 2026-04-28 UTTC 真实抓包验证 J519 主运行点位与 JointDetialTraj 重采样规则一致。
|
||||
/// </summary>
|
||||
public sealed class UttcJ519GoldenTests
|
||||
{
|
||||
private const int JointCount = 6;
|
||||
private const int RobotJ519Port = 60015;
|
||||
private const double ServoPeriodSeconds = 0.008;
|
||||
|
||||
public static IEnumerable<object[]> SpeedSweepCases()
|
||||
{
|
||||
yield return ["2026042802-0.5.pcap", 0.5, 1851, 14.800309];
|
||||
yield return ["2026042802-0.7.pcap", 0.7, 1322, 10.568313];
|
||||
yield return ["2026042802-1.pcap", 1.0, 926, 7.400125];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed=0.5/0.7/1.0 三份真实抓包都符合当前运行时采用的发送点生成规则。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[MemberData(nameof(SpeedSweepCases))]
|
||||
public void CapturedJ519Run_ReplaysJointDetailTrajectoryWithSpeedRatio(
|
||||
string pcapFileName,
|
||||
double speedRatio,
|
||||
int expectedPointCount,
|
||||
double expectedSendDurationSeconds)
|
||||
{
|
||||
var repositoryRoot = FindRepositoryRoot();
|
||||
var pcapPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", pcapFileName);
|
||||
var jointDetailPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
|
||||
var packets = ParsePcapUdp(pcapPath);
|
||||
var hostPort = DetectHostJ519Port(packets);
|
||||
var commands = ParseJ519Commands(packets, hostPort);
|
||||
var responses = ParseJ519Responses(packets, hostPort);
|
||||
var responseSegment = LongestStatusSegment(responses, status: 15);
|
||||
var jointRows = ReadJointDetail(jointDetailPath);
|
||||
var expected = GenerateExpectedPoints(jointRows, speedRatio);
|
||||
|
||||
var commandBySequence = new Dictionary<uint, CapturedJ519Command>();
|
||||
var duplicateSequenceCount = 0;
|
||||
foreach (var command in commands)
|
||||
{
|
||||
if (!commandBySequence.TryAdd(command.Sequence, command))
|
||||
{
|
||||
duplicateSequenceCount++;
|
||||
}
|
||||
}
|
||||
|
||||
var (startSequence, windowRms) = FindBestCommandWindow(commandBySequence, expected, responseSegment, searchRadius: 32);
|
||||
var actualRun = Enumerable
|
||||
.Range(0, expected.Length)
|
||||
.Select(index => commandBySequence[startSequence + (uint)index])
|
||||
.ToArray();
|
||||
var comparison = Compare(actualRun, expected);
|
||||
|
||||
Assert.Equal(464, jointRows.Length);
|
||||
Assert.Equal(expectedPointCount, expected.Length);
|
||||
Assert.Equal(expectedPointCount, actualRun.Length);
|
||||
Assert.Equal(0, duplicateSequenceCount);
|
||||
Assert.Equal(17, comparison.IoSetPulses);
|
||||
Assert.Equal(17, comparison.IoClearFrames);
|
||||
Assert.Equal(
|
||||
new ushort[] { 10, 12, 14 },
|
||||
actualRun
|
||||
.Where(static command => command.WriteIoMask != 0)
|
||||
.Select(static command => command.WriteIoMask)
|
||||
.Distinct()
|
||||
.Order()
|
||||
.ToArray());
|
||||
Assert.True(responseSegment.Length >= expectedPointCount - 1, $"status=15 segment too short: {responseSegment.Length}");
|
||||
Assert.InRange((long)responseSegment[0].Sequence - startSequence, 2, 8);
|
||||
Assert.All(actualRun, static command => Assert.Equal(0, command.LastData));
|
||||
for (var index = 0; index < actualRun.Length; index++)
|
||||
{
|
||||
Assert.Equal(startSequence + (uint)index, actualRun[index].Sequence);
|
||||
}
|
||||
|
||||
Assert.True(windowRms < 0.012, $"J519 global RMS {windowRms:F9} deg exceeds tolerance.");
|
||||
Assert.True(comparison.GlobalMaxAbsDeg < 0.07, $"J519 max abs diff {comparison.GlobalMaxAbsDeg:F9} deg exceeds tolerance.");
|
||||
|
||||
var sendDuration = actualRun[^1].TimeRelativeSeconds - actualRun[0].TimeRelativeSeconds;
|
||||
Assert.InRange(sendDuration, expectedSendDurationSeconds - 0.04, expectedSendDurationSeconds + 0.04);
|
||||
}
|
||||
|
||||
private static string FindRepositoryRoot()
|
||||
{
|
||||
var directory = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (directory is not null)
|
||||
{
|
||||
var jointDetailPath = Path.Combine(directory.FullName, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
|
||||
if (File.Exists(jointDetailPath))
|
||||
{
|
||||
return directory.FullName;
|
||||
}
|
||||
|
||||
directory = directory.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Cannot locate repository root containing Rvbust/uttc-20260428/Data/JointDetialTraj.txt.");
|
||||
}
|
||||
|
||||
private static IReadOnlyList<UdpPacket> ParsePcapUdp(string path)
|
||||
{
|
||||
using var stream = File.OpenRead(path);
|
||||
var header = new byte[24];
|
||||
stream.ReadExactly(header);
|
||||
|
||||
var magic = header.AsSpan(0, 4);
|
||||
var bigEndian = false;
|
||||
var timestampScale = 1_000_000.0;
|
||||
if (magic.SequenceEqual(new byte[] { 0xd4, 0xc3, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0xc3, 0xd4 }))
|
||||
{
|
||||
bigEndian = true;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0x4d, 0x3c, 0xb2, 0xa1 }))
|
||||
{
|
||||
bigEndian = false;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0x3c, 0x4d }))
|
||||
{
|
||||
bigEndian = true;
|
||||
timestampScale = 1_000_000_000.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidDataException($"Unsupported pcap magic: {Convert.ToHexString(header.AsSpan(0, 4))}");
|
||||
}
|
||||
|
||||
var linkType = ReadUInt32(header.AsSpan(20, 4), bigEndian);
|
||||
if (linkType != 1)
|
||||
{
|
||||
throw new InvalidDataException($"Only Ethernet pcap is supported, got linktype {linkType}.");
|
||||
}
|
||||
|
||||
var packets = new List<UdpPacket>();
|
||||
var recordHeader = new byte[16];
|
||||
double? firstTimestamp = null;
|
||||
var frameNumber = 0;
|
||||
while (ReadFullOrEnd(stream, recordHeader))
|
||||
{
|
||||
frameNumber++;
|
||||
var tsSec = ReadUInt32(recordHeader.AsSpan(0, 4), bigEndian);
|
||||
var tsFraction = ReadUInt32(recordHeader.AsSpan(4, 4), bigEndian);
|
||||
var includedLength = ReadUInt32(recordHeader.AsSpan(8, 4), bigEndian);
|
||||
var packet = new byte[includedLength];
|
||||
stream.ReadExactly(packet);
|
||||
|
||||
var timestamp = tsSec + (tsFraction / timestampScale);
|
||||
firstTimestamp ??= timestamp;
|
||||
var udp = ParseEthernetIpv4Udp(packet, frameNumber, timestamp - firstTimestamp.Value);
|
||||
if (udp is not null)
|
||||
{
|
||||
packets.Add(udp);
|
||||
}
|
||||
}
|
||||
|
||||
return packets;
|
||||
}
|
||||
|
||||
private static bool ReadFullOrEnd(Stream stream, byte[] buffer)
|
||||
{
|
||||
var offset = 0;
|
||||
while (offset < buffer.Length)
|
||||
{
|
||||
var read = stream.Read(buffer, offset, buffer.Length - offset);
|
||||
if (read == 0)
|
||||
{
|
||||
if (offset == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
throw new EndOfStreamException("Truncated pcap record header.");
|
||||
}
|
||||
|
||||
offset += read;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static UdpPacket? ParseEthernetIpv4Udp(byte[] packet, int frameNumber, double timeRelativeSeconds)
|
||||
{
|
||||
if (packet.Length < 14)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var offset = 14;
|
||||
var etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(12, 2));
|
||||
if (etherType == 0x8100)
|
||||
{
|
||||
if (packet.Length < 18)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(16, 2));
|
||||
offset = 18;
|
||||
}
|
||||
|
||||
if (etherType != 0x0800 || packet.Length < offset + 20)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var versionIhl = packet[offset];
|
||||
var version = versionIhl >> 4;
|
||||
var ihl = (versionIhl & 0x0f) * 4;
|
||||
if (version != 4 || ihl < 20 || packet.Length < offset + ihl)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var protocol = packet[offset + 9];
|
||||
if (protocol != 17)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var totalLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(offset + 2, 2));
|
||||
var udpOffset = offset + ihl;
|
||||
if (packet.Length < udpOffset + 8)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var sourcePort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset, 2));
|
||||
var destinationPort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 2, 2));
|
||||
var udpLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 4, 2));
|
||||
var payloadOffset = udpOffset + 8;
|
||||
var payloadLength = Math.Max(0, Math.Min(udpLength - 8, totalLength - ihl - 8));
|
||||
if (packet.Length < payloadOffset + payloadLength)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return new UdpPacket(
|
||||
frameNumber,
|
||||
timeRelativeSeconds,
|
||||
sourcePort,
|
||||
destinationPort,
|
||||
packet.AsSpan(payloadOffset, payloadLength).ToArray());
|
||||
}
|
||||
|
||||
private static ushort DetectHostJ519Port(IEnumerable<UdpPacket> packets)
|
||||
{
|
||||
return packets
|
||||
.Where(static packet => packet.DestinationPort == RobotJ519Port && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.GroupBy(static packet => packet.SourcePort)
|
||||
.OrderByDescending(static group => group.Count())
|
||||
.Select(static group => group.Key)
|
||||
.First();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command[] ParseJ519Commands(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == hostPort
|
||||
&& packet.DestinationPort == RobotJ519Port
|
||||
&& packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
|
||||
.Select(ParseCommand)
|
||||
.Where(static command => command is not null)
|
||||
.Cast<CapturedJ519Command>()
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static CapturedJ519Command? ParseCommand(UdpPacket packet)
|
||||
{
|
||||
var payload = packet.Payload;
|
||||
var messageType = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x00, 4));
|
||||
var version = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x04, 4));
|
||||
if (messageType != 1 || version != 1)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var targets = new double[9];
|
||||
for (var index = 0; index < targets.Length; index++)
|
||||
{
|
||||
targets[index] = BinaryPrimitives.ReadSingleBigEndian(payload.AsSpan(0x1c + (index * 4), 4));
|
||||
}
|
||||
|
||||
return new CapturedJ519Command(
|
||||
packet.FrameNumber,
|
||||
packet.TimeRelativeSeconds,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x08, 4)),
|
||||
payload[0x0c],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x16, 2)),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x18, 2)),
|
||||
targets);
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] ParseJ519Responses(IEnumerable<UdpPacket> packets, ushort hostPort)
|
||||
{
|
||||
return packets
|
||||
.Where(packet =>
|
||||
packet.SourcePort == RobotJ519Port
|
||||
&& packet.DestinationPort == hostPort
|
||||
&& packet.Payload.Length == FanucJ519Protocol.ResponsePacketLength)
|
||||
.Select(packet => FanucJ519Protocol.ParseResponse(packet.Payload))
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static FanucJ519Response[] LongestStatusSegment(IEnumerable<FanucJ519Response> responses, byte status)
|
||||
{
|
||||
var best = new List<FanucJ519Response>();
|
||||
var current = new List<FanucJ519Response>();
|
||||
foreach (var response in responses)
|
||||
{
|
||||
if (response.Status == status)
|
||||
{
|
||||
current.Add(response);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (current.Count > best.Count)
|
||||
{
|
||||
best = current;
|
||||
}
|
||||
|
||||
current = [];
|
||||
}
|
||||
|
||||
return (current.Count > best.Count ? current : best).ToArray();
|
||||
}
|
||||
|
||||
private static JointRow[] ReadJointDetail(string path)
|
||||
{
|
||||
return File.ReadLines(path)
|
||||
.Where(static line => !string.IsNullOrWhiteSpace(line) && !line.TrimStart().StartsWith('#'))
|
||||
.Select(static line =>
|
||||
{
|
||||
var values = line
|
||||
.Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries)
|
||||
.Select(value => double.Parse(value, CultureInfo.InvariantCulture))
|
||||
.ToArray();
|
||||
return new JointRow(values[0], values.Skip(1).Take(JointCount).ToArray());
|
||||
})
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
private static ExpectedPoint[] GenerateExpectedPoints(IReadOnlyList<JointRow> rows, double speedRatio)
|
||||
{
|
||||
var durationSeconds = rows[^1].TimeSeconds;
|
||||
var trajectoryStepSeconds = ServoPeriodSeconds * speedRatio;
|
||||
var sampleCount = (int)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
|
||||
var points = new ExpectedPoint[sampleCount];
|
||||
var segmentIndex = 0;
|
||||
|
||||
for (var index = 0; index < sampleCount; index++)
|
||||
{
|
||||
var trajectoryTime = Math.Min(index * trajectoryStepSeconds, durationSeconds);
|
||||
var jointsRad = Interpolate(rows, trajectoryTime, ref segmentIndex);
|
||||
points[index] = new ExpectedPoint(
|
||||
index,
|
||||
trajectoryTime,
|
||||
jointsRad.Select(static value => value * 180.0 / Math.PI).ToArray());
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
private static double[] Interpolate(IReadOnlyList<JointRow> rows, double trajectoryTime, ref int segmentIndex)
|
||||
{
|
||||
if (rows.Count == 1 || trajectoryTime <= rows[0].TimeSeconds)
|
||||
{
|
||||
return rows[0].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = rows.Count - 1;
|
||||
if (trajectoryTime >= rows[lastIndex].TimeSeconds)
|
||||
{
|
||||
return rows[lastIndex].JointsRad.ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && rows[segmentIndex + 1].TimeSeconds < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = rows[segmentIndex];
|
||||
var end = rows[segmentIndex + 1];
|
||||
var duration = end.TimeSeconds - start.TimeSeconds;
|
||||
var alpha = duration <= 0.0 ? 0.0 : (trajectoryTime - start.TimeSeconds) / duration;
|
||||
var joints = new double[JointCount];
|
||||
for (var index = 0; index < joints.Length; index++)
|
||||
{
|
||||
joints[index] = start.JointsRad[index] + ((end.JointsRad[index] - start.JointsRad[index]) * alpha);
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
private static (uint StartSequence, double RmsDeg) FindBestCommandWindow(
|
||||
IReadOnlyDictionary<uint, CapturedJ519Command> commandBySequence,
|
||||
IReadOnlyList<ExpectedPoint> expected,
|
||||
IReadOnlyList<FanucJ519Response> responseSegment,
|
||||
int searchRadius)
|
||||
{
|
||||
if (responseSegment.Count == 0)
|
||||
{
|
||||
throw new InvalidDataException("No status=15 response segment found.");
|
||||
}
|
||||
|
||||
var responseStartSequence = (long)responseSegment[0].Sequence;
|
||||
uint? bestStartSequence = null;
|
||||
var bestRms = double.PositiveInfinity;
|
||||
|
||||
for (var startSequence = responseStartSequence - searchRadius; startSequence <= responseStartSequence + searchRadius; startSequence++)
|
||||
{
|
||||
if (startSequence < 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var differences = new List<double>(expected.Count * JointCount);
|
||||
var completeWindow = true;
|
||||
for (var index = 0; index < expected.Count; index++)
|
||||
{
|
||||
var sequence = (uint)(startSequence + index);
|
||||
if (!commandBySequence.TryGetValue(sequence, out var command))
|
||||
{
|
||||
completeWindow = false;
|
||||
break;
|
||||
}
|
||||
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
differences.Add(command.TargetDegrees[joint] - expected[index].JointsDeg[joint]);
|
||||
}
|
||||
}
|
||||
|
||||
if (!completeWindow)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var rms = Rms(differences);
|
||||
if (rms < bestRms)
|
||||
{
|
||||
bestRms = rms;
|
||||
bestStartSequence = (uint)startSequence;
|
||||
}
|
||||
}
|
||||
|
||||
if (bestStartSequence is null)
|
||||
{
|
||||
throw new InvalidDataException("No complete command window found near the status=15 response segment.");
|
||||
}
|
||||
|
||||
return (bestStartSequence.Value, bestRms);
|
||||
}
|
||||
|
||||
private static ComparisonSummary Compare(IReadOnlyList<CapturedJ519Command> actual, IReadOnlyList<ExpectedPoint> expected)
|
||||
{
|
||||
var differences = new List<double>(actual.Count * JointCount);
|
||||
var maxAbs = 0.0;
|
||||
for (var index = 0; index < actual.Count; index++)
|
||||
{
|
||||
for (var joint = 0; joint < JointCount; joint++)
|
||||
{
|
||||
var difference = actual[index].TargetDegrees[joint] - expected[index].JointsDeg[joint];
|
||||
differences.Add(difference);
|
||||
maxAbs = Math.Max(maxAbs, Math.Abs(difference));
|
||||
}
|
||||
}
|
||||
|
||||
var ioSetPulses = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue != 0);
|
||||
var ioClearFrames = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue == 0);
|
||||
return new ComparisonSummary(Rms(differences), maxAbs, ioSetPulses, ioClearFrames);
|
||||
}
|
||||
|
||||
private static double Rms(IEnumerable<double> values)
|
||||
{
|
||||
var sum = 0.0;
|
||||
var count = 0;
|
||||
foreach (var value in values)
|
||||
{
|
||||
sum += value * value;
|
||||
count++;
|
||||
}
|
||||
|
||||
return count == 0 ? 0.0 : Math.Sqrt(sum / count);
|
||||
}
|
||||
|
||||
private static uint ReadUInt32(ReadOnlySpan<byte> value, bool bigEndian)
|
||||
{
|
||||
return bigEndian ? BinaryPrimitives.ReadUInt32BigEndian(value) : BinaryPrimitives.ReadUInt32LittleEndian(value);
|
||||
}
|
||||
|
||||
private sealed record UdpPacket(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
ushort SourcePort,
|
||||
ushort DestinationPort,
|
||||
byte[] Payload);
|
||||
|
||||
private sealed record CapturedJ519Command(
|
||||
int FrameNumber,
|
||||
double TimeRelativeSeconds,
|
||||
uint Sequence,
|
||||
byte LastData,
|
||||
ushort WriteIoMask,
|
||||
ushort WriteIoValue,
|
||||
IReadOnlyList<double> TargetDegrees);
|
||||
|
||||
private sealed record JointRow(double TimeSeconds, IReadOnlyList<double> JointsRad);
|
||||
|
||||
private sealed record ExpectedPoint(int Index, double TrajectoryTimeSeconds, IReadOnlyList<double> JointsDeg);
|
||||
|
||||
private sealed record ComparisonSummary(double GlobalRmsDeg, double GlobalMaxAbsDeg, int IoSetPulses, int IoClearFrames);
|
||||
}
|
||||
@@ -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,160 @@
|
||||
using System.Net;
|
||||
using Microsoft.AspNetCore.Mvc.Testing;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// 验证 `wwwroot` 静态调试页和调试配置 API 的基础内容契约。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 调试页自身是静态 HTML，真正的 Swagger 地址由配置 API 下发；
|
||||
/// 当 Swagger 关闭时，配置 API 返回 404，前端据此显示不可用状态。
|
||||
/// </remarks>
|
||||
public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||
{
|
||||
private readonly FlyshotServerFactory _factory = factory;
|
||||
|
||||
/// <summary>
|
||||
/// `debug.html` 应当作为可直接调试的静态页面暴露。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebugHtml_ReturnsConsoleStaticPage()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/debug.html");
|
||||
|
||||
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);
|
||||
Assert.DoesNotContain("__SWAGGER_JSON_URL__", html, StringComparison.Ordinal);
|
||||
Assert.Contains("/assets/debug.css", html, StringComparison.Ordinal);
|
||||
Assert.Contains("/assets/debug.js", html, StringComparison.Ordinal);
|
||||
|
||||
using var scriptResponse = await client.GetAsync("/assets/debug.js");
|
||||
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
|
||||
|
||||
var script = await scriptResponse.Content.ReadAsStringAsync();
|
||||
Assert.Contains("/api/debug/config", script, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当 Swagger 启用时，调试配置 API 应当返回实际 Swagger JSON 地址。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebugConfig_WhenSwaggerEnabled_ReturnsSwaggerJsonUrl()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/api/debug/config");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
|
||||
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||
using var document = await System.Text.Json.JsonDocument.ParseAsync(responseStream);
|
||||
|
||||
Assert.Equal("/swagger/v1/swagger.json", document.RootElement.GetProperty("swaggerJsonUrl").GetString());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当 Swagger 关闭时，调试配置 API 应当与 Swagger UI 同步下线（404）。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetDebugConfig_WhenSwaggerDisabled_ReturnsNotFound()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(false);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/api/debug/config");
|
||||
|
||||
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 GetStatusHtml_LinksToDebugConsole()
|
||||
{
|
||||
using var configuredFactory = CreateFactoryWithSwaggerEnabled(true);
|
||||
using var client = configuredFactory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/status.html");
|
||||
|
||||
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||
var html = await response.Content.ReadAsStringAsync();
|
||||
|
||||
Assert.Contains("href=\"/debug.html\"", 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,290 @@
|
||||
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,
|
||||
wait = 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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
using Flyshot.Server.Host.Middleware;
|
||||
using Microsoft.AspNetCore.Http;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Server.IntegrationTests;
|
||||
|
||||
/// <summary>
|
||||
/// HTTP 请求响应日志中间件测试。
|
||||
/// </summary>
|
||||
public sealed class RequestResponseLoggingMiddlewareTests
|
||||
{
|
||||
/// <summary>
|
||||
/// 高频状态快照路径命中忽略前缀时，不应写入请求和响应日志。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task InvokeAsync_WhenPathMatchesIgnoredPrefix_DoesNotWriteRequestResponseLogs()
|
||||
{
|
||||
var logger = new CapturingLogger<RequestResponseLoggingMiddleware>();
|
||||
var nextWasCalled = false;
|
||||
var middleware = new RequestResponseLoggingMiddleware(
|
||||
async context =>
|
||||
{
|
||||
nextWasCalled = true;
|
||||
context.Response.StatusCode = StatusCodes.Status200OK;
|
||||
await context.Response.WriteAsync("ok");
|
||||
},
|
||||
logger);
|
||||
var context = new DefaultHttpContext();
|
||||
context.Request.Method = HttpMethods.Get;
|
||||
context.Request.Path = "/api/status/snapshot/current";
|
||||
context.Response.Body = new MemoryStream();
|
||||
|
||||
await middleware.InvokeAsync(context);
|
||||
|
||||
Assert.True(nextWasCalled);
|
||||
Assert.Empty(logger.Entries);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 捕获中间件写出的日志条目，避免测试依赖真实 NLog 目标。
|
||||
/// </summary>
|
||||
private sealed class CapturingLogger<T> : ILogger<T>
|
||||
{
|
||||
/// <summary>
|
||||
/// 已捕获的日志条目。
|
||||
/// </summary>
|
||||
public List<LogEntry> Entries { get; } = new();
|
||||
|
||||
/// <inheritdoc />
|
||||
public IDisposable? BeginScope<TState>(TState state)
|
||||
where TState : notnull
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public bool IsEnabled(LogLevel logLevel)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Log<TState>(
|
||||
LogLevel logLevel,
|
||||
EventId eventId,
|
||||
TState state,
|
||||
Exception? exception,
|
||||
Func<TState, Exception?, string> formatter)
|
||||
{
|
||||
Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 测试用日志条目。
|
||||
/// </summary>
|
||||
/// <param name="Level">日志级别。</param>
|
||||
/// <param name="Message">格式化后的日志消息。</param>
|
||||
private sealed record LogEntry(LogLevel Level, string Message);
|
||||
}
|
||||
103
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
103
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
@@ -0,0 +1,103 @@
|
||||
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>
|
||||
/// 验证状态页作为 wwwroot 静态 HTML 暴露，并引用状态快照 API。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatusHtml_ReturnsMonitoringStaticPage()
|
||||
{
|
||||
using var client = factory.CreateClient();
|
||||
|
||||
using var response = await client.GetAsync("/status.html");
|
||||
|
||||
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("/assets/status.css", html, StringComparison.Ordinal);
|
||||
Assert.Contains("/assets/status.js", html, StringComparison.Ordinal);
|
||||
|
||||
using var scriptResponse = await client.GetAsync("/assets/status.js");
|
||||
Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode);
|
||||
|
||||
var script = await scriptResponse.Content.ReadAsStringAsync();
|
||||
Assert.Contains("/api/status/snapshot", script, 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());
|
||||
var uploadedTrajectories = root.GetProperty("uploadedTrajectories")
|
||||
.EnumerateArray()
|
||||
.Select(static value => value.GetString())
|
||||
.ToArray();
|
||||
Assert.Contains("20251015", uploadedTrajectories);
|
||||
Assert.Contains("UTTC_MS11", uploadedTrajectories);
|
||||
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