From a78e6761cba395a190bed8af92489af747389860 Mon Sep 17 00:00:00 2001 From: "yunxiao.zhu" Date: Fri, 24 Apr 2026 21:26:25 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat(fanuc):=20=E6=B7=BB=E5=8A=A0?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E7=BC=96=E8=A7=A3=E7=A0=81=E4=B8=8E=E7=8A=B6?= =?UTF-8?q?=E6=80=81=E9=A1=B5"=20-m=20"*=20=E5=9B=BA=E5=8C=96=2010010=20?= =?UTF-8?q?=E7=8A=B6=E6=80=81=E5=B8=A7=E3=80=8110012=20=E5=91=BD=E4=BB=A4?= =?UTF-8?q?=E5=B8=A7=E5=92=8C=2060015=20J519=20=E5=8C=85=E7=BC=96=E8=A7=A3?= =?UTF-8?q?=E7=A0=81=20=20=20*=20=E6=89=A9=E5=B1=95=20ControllerClient=20?= =?UTF-8?q?=E5=85=BC=E5=AE=B9=E5=B1=82=E7=9A=84=E6=89=A7=E8=A1=8C=E5=8F=82?= =?UTF-8?q?=E6=95=B0=E5=92=8C=E8=BF=90=E8=A1=8C=E6=97=B6=E7=BC=96=E6=8E=92?= =?UTF-8?q?=20=20=20*=20=E6=96=B0=E5=A2=9E=20/status=20=E9=A1=B5=E9=9D=A2?= =?UTF-8?q?=E4=B8=8E=20/api/status/snapshot=20=E7=8A=B6=E6=80=81=E5=BF=AB?= =?UTF-8?q?=E7=85=A7=E6=8E=A5=E5=8F=A3=20=20=20*=20=E8=A1=A5=E5=85=85=20FA?= =?UTF-8?q?NUC=20=E5=8D=8F=E8=AE=AE=E3=80=81=E5=AE=A2=E6=88=B7=E7=AB=AF?= =?UTF-8?q?=E5=92=8C=E7=8A=B6=E6=80=81=E6=8E=A5=E5=8F=A3=E7=9A=84=E6=9C=80?= =?UTF-8?q?=E5=B0=8F=E9=AA=8C=E8=AF=81=E6=B5=8B=E8=AF=95=20=20=20*=20?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=20README=E3=80=81=E5=85=BC=E5=AE=B9=E8=A6=81?= =?UTF-8?q?=E6=B1=82=E5=92=8C=E7=9C=9F=E6=9C=BA=20Socket=20=E9=80=9A?= =?UTF-8?q?=E4=BF=A1=E5=AE=9E=E7=8E=B0=E8=AE=A1=E5=88=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 7 +- ...r-client-api-compatibility-requirements.md | 7 +- docs/fanuc-socket-implementation-plan.md | 162 ++++++++ ...026-04-24-minimal-runtime-orchestration.md | 5 +- .../ControllerClientCompatService.cs | 148 ++++++- .../ControllerClientTrajectoryOrchestrator.cs | 164 +++++++- .../FlyshotExecutionOptions.cs | 46 +++ .../IControllerClientCompatService.cs | 65 ++- .../TrajectoryExecutionOptions.cs | 28 ++ .../FanucControllerRuntime.cs | 163 +++++++- .../Protocol/FanucCommandClient.cs | 250 ++++++++++++ .../Protocol/FanucCommandProtocol.cs | 272 ++++++++++++ .../Protocol/FanucJ519Client.cs | 297 ++++++++++++++ .../Protocol/FanucJ519Protocol.cs | 386 ++++++++++++++++++ .../Protocol/FanucStateClient.cs | 188 +++++++++ .../Protocol/FanucStateProtocol.cs | 127 ++++++ .../Controllers/LegacyHttpApiController.cs | 352 +++++++++++++++- .../Controllers/StatusController.cs | 385 +++++++++++++++++ .../FanucCommandClientTests.cs | 179 ++++++++ .../FanucJ519ClientTests.cs | 180 ++++++++ .../Flyshot.Core.Tests/FanucProtocolTests.cs | 116 ++++++ .../FanucStateClientTests.cs | 138 +++++++ .../RuntimeOrchestrationTests.cs | 4 +- .../LegacyHttpApiCompatibilityTests.cs | 68 ++- .../StatusEndpointTests.cs | 91 +++++ 25 files changed, 3773 insertions(+), 55 deletions(-) create mode 100644 docs/fanuc-socket-implementation-plan.md create mode 100644 src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs create mode 100644 src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs create mode 100644 src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs create mode 100644 src/Flyshot.Server.Host/Controllers/StatusController.cs create mode 100644 tests/Flyshot.Core.Tests/FanucCommandClientTests.cs create mode 100644 tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs create mode 100644 tests/Flyshot.Core.Tests/FanucProtocolTests.cs create mode 100644 tests/Flyshot.Core.Tests/FanucStateClientTests.cs create mode 100644 tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs diff --git a/README.md b/README.md index bf1b1df..066d2d4 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ - 这是长期运行的无头后台服务，不是 GUI 桌面程序。 - 第一版仅面向当前现场组合，后续再扩展机型与控制柜适配。 - 当前仓库内已经移除宿主中的 `50001/TCP+JSON` 监听实现；现阶段只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。 -- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 最小链路；当前 `Flyshot.Runtime.Fanuc` 仍是状态型骨架，尚未接通真实 `10010 / 10012 / 60015` 通讯。 +- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 最小链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态；`Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，但运行时仍是状态型骨架，尚未完成真机 Socket 联调。 - `50001/TCP+JSON` 的真实兼容入口如果后续需要恢复，必须基于 `docs/controller-client-api-compatibility-requirements.md` 与 `docs/controller-client-api-reverse-engineering.md` 重新评估，而不是直接把旧的 TCP 网关方向接回宿主。 开发约定： @@ -32,5 +32,6 @@ - [x] 落地配置兼容与机器人模型解析 - [x] 落地轨迹规划与飞拍触发时间轴 - [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入最小 FANUC 运行时骨架 -- [ ] 落地真实 `10010 / 10012 / 60015` FANUC 通讯 -- [ ] 落地 Web 状态页 +- [x] 落地 Web 状态页 +- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码 +- [ ] 落地真实 `10010 / 10012 / 60015` FANUC Socket 通讯与现场联调 diff --git a/docs/controller-client-api-compatibility-requirements.md b/docs/controller-client-api-compatibility-requirements.md index 015cda2..5d0768c 100644 --- a/docs/controller-client-api-compatibility-requirements.md +++ b/docs/controller-client-api-compatibility-requirements.md @@ -131,5 +131,10 @@ - `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务，不启动 `50001/TCP+JSON` 监听。 - `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹，再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。 - `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹，通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。 -- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态；真实 `10010 / 10012 / 60015` Socket 通讯尚未落地。 +- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)` 与 `ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数，并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体。 +- `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。 +- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试。 +- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态；真实 `10010 / 10012 / 60015` Socket 通讯与现场联调尚未落地。 +- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。 - `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。 +- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。 diff --git a/docs/fanuc-socket-implementation-plan.md b/docs/fanuc-socket-implementation-plan.md new file mode 100644 index 0000000..abfab1d --- /dev/null +++ b/docs/fanuc-socket-implementation-plan.md @@ -0,0 +1,162 @@ +# FANUC 真机协议 Socket 通信层实现计划 + +## 上下文 + +当前 `flyshot-replacement` 项目已完成： +- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`） +- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过） +- HTTP 兼容层控制器和状态监控页 +- 轨迹规划与飞拍触发编排层 + +**缺失的关键环节**：`FanucControllerRuntime` 仍是纯内存状态桩，没有实际 Socket 通信。`Connect()` 只记录 IP，`ExecuteTrajectory()` 只修改内存变量，`GetJointPositions()` 返回的是上一次写入值而非真实控制器反馈。 + +## 目标 + +将 `FanucControllerRuntime` 从内存桩改造为具备真实 FANUC R30iB 通信能力的运行时，使 HTTP 层的每个指令真正下发到控制柜。 + +## 架构设计 + +### 分层结构 + +``` +LegacyHttpApiController / StatusController (HTTP 适配层，保持不动) + ↓ 调用同步接口 +IControllerRuntime / ControllerClientCompatService (兼容层，保持不动) + ↓ 调用同步接口 +FanucControllerRuntime (改造：从内存桩 → 委托给三个 Socket 客户端) + ↓ 内部持有并调度 +FanucCommandClient (TCP 10012，Req/Res 同步命令通道) +FanucStateClient (TCP 10010，持续接收状态帧后台循环) +FanucJ519Client (UDP 60015，8ms 周期发送 + 接收响应) + ↓ 使用现有编解码 +FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改动) +``` + +### 关键设计决策 + +1. **接口保持同步**：`IControllerRuntime` 现有 18 个方法全为同步签名。内部 Socket I/O 采用 `Task` + `.GetAwaiter().GetResult()` 短时间阻塞，或后台线程 + 锁同步状态快照。避免一次性推翻整个兼容层。 + +2. **三个独立客户端**：每条物理通道一个类，各自管理连接生命周期，便于单独测试和故障定位。 + +3. **状态通道后台循环**：`FanucStateClient` 内部启动 `Task` 持续 `ReadAsync(90)`，解析状态帧后写入线程安全的 `ControllerStateSnapshot` 缓存。 + +4. **J519 周期发送器**：`FanucJ519Client` 内部用 `PeriodicTimer` 或 `Task.Delay` 实现约 8ms 周期的发送循环。命令通过线程安全的队列/缓冲区注入。 + +5. **RVBUSTSM 程序生命周期隐式管理**：`EnableRobot()` 时自动走 `StopProg→Reset→GetProgStatus→StartProg("RVBUSTSM")` 序列（与抓包一致）。`DisableRobot()` 时发送 `StopProg`。 + +6. **连接顺序**：`Connect()` 按顺序建立三条通道 — 先 TCP 10010（状态），再 TCP 10012（命令），最后 UDP 60015（运动）。 + +## 实现步骤 + +### Phase 1: TCP 10012 命令客户端 + +**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs` + +职责： +- `Connect(string ip, int port = 10012)` — 建立 TcpClient 连接 +- `SendCommandAsync(uint messageId, ReadOnlyMemory 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()` — 待解析请求/响应体格式 +- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 同上 +- `GetIoAsync()` / `SetIoAsync()` — 同上 + +**测试**：`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()` +- 将解析结果写入线程安全的最新状态缓存 +- `GetLatestSnapshot()` — 返回最近一次解析的状态帧 +- `Disconnect()` — 取消后台循环并关闭连接 + +**测试**：`tests/Flyshot.Core.Tests/FanucStateClientTests.cs` +- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析 + +### Phase 3: UDP 60015 J519 运动客户端 + +**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs` + +职责： +- `Connect(string ip, int port = 60015)` — 创建 UdpClient +- 发送 init packet (`PackInitPacket()`) +- 内部启动发送循环（约 8ms 周期） +- `UpdateCommand(FanucJ519Command command)` — 原子更新下一周期要发送的命令 +- `StartMotion()` — 启动发送循环 +- `StopMotion()` — 发送 end packet，停止循环 +- 接收线程：持续 `ReceiveAsync()` 解析 132B 响应，更新反馈状态 +- `Disconnect()` — 清理 + +**测试**：`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs` +- 用本地 UDP socket 模拟控制器收发 + +### Phase 4: 重写 FanucControllerRuntime + +**改造文件**：`src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` + +将当前内存桩替换为真实运行时： + +- 持有三个客户端实例：`FanucCommandClient`、`FanucStateClient`、`FanucJ519Client` +- `Connect(robotIp)` — 顺序连接 10010 → 10012 → 60015 +- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519 +- `DisableRobot()` — 停止 J519，发送 StopProg +- `Disconnect()` — 断开三条通道 +- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点通过 J519 逐帧发送 +- `StopMove()` — 立即停止 J519 发送循环 +- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值 +- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取 +- `SetTcp()` / `SetSpeedRatio()` / `SetIo()` — 通过命令通道发送 + +### Phase 5: 端到端集成测试 + +**改造/新建测试**： +- `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs` — 补充真实连接流程（可用本地模拟器） +- `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` — 用本地 TCP/UDP 模拟器验证完整链路 + +**验证命令**： +```bash +cd flyshot-replacement +dotnet build FlyshotReplacement.sln -v minimal +dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal +dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal +``` + +## 风险与回退策略 + +1. **真机连接风险**：第一版 Socket 实现可能有超时/重连问题。`FanucControllerRuntime` 保留 `_simulationMode` 路径，仿真模式下仍走内存桩。 +2. **性能风险**：同步接口内部阻塞 Socket 可能影响 HTTP 并发。若实测有问题，后续将 `IControllerRuntime` 改为 async。 +3. **协议字段不完整风险**：TCP 10012 的 `GetTcp`/`SetTcp`/`GetIo`/`SetIo` 请求/响应体格式尚未完全逆向。先实现已知字段，留 TODO 标记待验证。 + +## 关键文件清单 + +| 文件 | 动作 | +|------|------| +| `src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs` | 新建 | +| `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs` | 新建 | +| `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs` | 新建 | +| `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` | 重写 | +| `tests/Flyshot.Core.Tests/FanucCommandClientTests.cs` | 新建 | +| `tests/Flyshot.Core.Tests/FanucStateClientTests.cs` | 新建 | +| `tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs` | 新建 | +| `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` | 新建 | + +## 下一步验证标准 + +- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接 +- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列 +- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点 +- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存 +- 现有 10 个集成测试和 25 个核心测试仍然通过 diff --git a/docs/superpowers/plans/2026-04-24-minimal-runtime-orchestration.md b/docs/superpowers/plans/2026-04-24-minimal-runtime-orchestration.md index 8aa9204..3babc26 100644 --- a/docs/superpowers/plans/2026-04-24-minimal-runtime-orchestration.md +++ b/docs/superpowers/plans/2026-04-24-minimal-runtime-orchestration.md @@ -261,8 +261,9 @@ Expected: PASS, with existing HTTP compatibility tests still green. ```markdown - [x] 落地最小 FANUC 运行时骨架 - [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime -- [ ] 落地真实 10010 / 10012 / 60015 通讯 -- [ ] 落地 Web 状态页 +- [x] 落地 Web 状态页 +- [x] 固化 10010 / 10012 / 60015 FANUC 基础协议帧编解码 +- [ ] 落地真实 10010 / 10012 / 60015 Socket 通讯与现场联调 ``` - [x] **Step 2: Run final build** diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index ced717c..ef55b1c 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -18,6 +18,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi private string? _configuredRobotName; private string? _connectedServerIp; private int _connectedServerPort; + private bool _showTcp = true; + private double _showTcpAxisLength = 0.1; + private int _showTcpAxisSize = 2; /// /// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。 @@ -79,6 +82,18 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } + /// + public string GetServerVersion() + { + return ServerVersion; + } + + /// + public string GetClientVersion() + { + return "flyshot-replacement-controller-client-compat/0.1.0"; + } + /// public void SetUpRobot(string robotName) { @@ -94,6 +109,41 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } + /// + public void SetUpRobotFromEnv(string envFile) + { + if (string.IsNullOrWhiteSpace(envFile)) + { + throw new ArgumentException("环境文件路径不能为空。", nameof(envFile)); + } + + throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。"); + } + + /// + 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; + } + } + /// public void SetActiveController(bool sim) { @@ -159,6 +209,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } + /// + public ControllerStateSnapshot GetControllerSnapshot() + { + return _runtime.GetSnapshot(); + } + /// public double GetSpeedRatio() { @@ -199,6 +255,29 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } + /// + public IReadOnlyList GetNearestIk(IReadOnlyList pose, IReadOnlyList 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 尚未接入逆解求解器。"); + } + } + /// public void SetTcp(double x, double y, double z) { @@ -242,9 +321,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } /// - public void ExecuteTrajectory(IReadOnlyList> waypoints) + public void ExecuteTrajectory(IReadOnlyList> waypoints, TrajectoryExecutionOptions? options = null) { ArgumentNullException.ThrowIfNull(waypoints); + options ??= new TrajectoryExecutionOptions(); if (waypoints.Count == 0) { throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints)); @@ -255,8 +335,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi var robot = RequireActiveRobot(); EnsureRuntimeEnabled(); - // 普通轨迹必须先通过 ICSP 规划，再把规划结果交给运行时执行。 - var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints); + // 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。 + var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options); var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions; _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions); } @@ -294,8 +374,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } /// - public void ExecuteTrajectoryByName(string name) + public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null) { + options ??= new FlyshotExecutionOptions(); if (string.IsNullOrWhiteSpace(name)) { throw new ArgumentException("轨迹名称不能为空。", nameof(name)); @@ -316,13 +397,68 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi throw new InvalidOperationException("FlyShot trajectory contains no waypoints."); } - // 已上传飞拍轨迹必须生成 shot timeline 后再交给运行时。 - var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory); + // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。 + var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options); + if (options.MoveToStart) + { + _runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions); + } + var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions; _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions); } } + /// + public void SaveTrajectoryInfo(string name, string method = "icsp") + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("轨迹名称不能为空。", nameof(name)); + } + + lock (_stateLock) + { + var robot = RequireActiveRobot(); + if (!_uploadedTrajectories.TryGetValue(name, out var trajectory)) + { + throw new InvalidOperationException("FlyShot trajectory does not exist."); + } + + // 当前阶段没有落地文件导出，先通过 saveTrajectory=true 走规划校验，避免静默接受非法参数。 + _ = _trajectoryOrchestrator.PlanUploadedFlyshot( + robot, + trajectory, + new FlyshotExecutionOptions(saveTrajectory: true, method: method)); + } + } + + /// + public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false) + { + if (string.IsNullOrWhiteSpace(name)) + { + throw new ArgumentException("轨迹名称不能为空。", nameof(name)); + } + + lock (_stateLock) + { + var robot = RequireActiveRobot(); + if (!_uploadedTrajectories.TryGetValue(name, out var trajectory)) + { + throw new InvalidOperationException("FlyShot trajectory does not exist."); + } + + var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( + robot, + trajectory, + new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory)); + + duration = bundle.Result.Duration; + return bundle.Result.IsValid; + } + } + /// public void DeleteTrajectory(string name) { diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index 1b19c2c..793f9af 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -12,6 +12,7 @@ public sealed class ControllerClientTrajectoryOrchestrator private readonly ICspPlanner _icspPlanner = new(); private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new(); private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver()); + private readonly Dictionary _flyshotCache = new(StringComparer.Ordinal); /// /// 对普通轨迹执行 ICSP 规划。 @@ -21,10 +22,12 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 包含规划轨迹、空触发时间轴和执行结果的结果包。 public PlannedExecutionBundle PlanOrdinaryTrajectory( RobotProfile robot, - IReadOnlyList> waypoints) + IReadOnlyList> waypoints, + TrajectoryExecutionOptions? options = null) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(waypoints); + options ??= new TrajectoryExecutionOptions(); var program = CreateProgram( name: "ordinary-trajectory", @@ -33,14 +36,16 @@ public sealed class ControllerClientTrajectoryOrchestrator offsetValues: Enumerable.Repeat(0, waypoints.Count), addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty())); + var method = ParseOrdinaryMethod(options.Method); var request = new TrajectoryRequest( robot: robot, program: program, - method: PlanningMethod.Icsp); + method: method, + saveTrajectoryArtifacts: options.SaveTrajectory); - var plannedTrajectory = _icspPlanner.Plan(request); + var plannedTrajectory = PlanByMethod(request, method); var shotTimeline = new ShotTimeline(Array.Empty(), Array.Empty()); - var result = CreateResult(plannedTrajectory, shotTimeline); + var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false); return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); } @@ -51,10 +56,14 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 当前机器人配置。 /// 兼容层保存的上传轨迹。 /// 包含规划轨迹、触发时间轴和执行结果的结果包。 - public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded) + public PlannedExecutionBundle PlanUploadedFlyshot( + RobotProfile robot, + ControllerClientCompatUploadedTrajectory uploaded, + FlyshotExecutionOptions? options = null) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(uploaded); + options ??= new FlyshotExecutionOptions(); var program = CreateProgram( name: uploaded.Name, @@ -63,19 +72,152 @@ public sealed class ControllerClientTrajectoryOrchestrator offsetValues: uploaded.OffsetValues, addressGroups: uploaded.AddressGroups); + var method = ParseFlyshotMethod(options.Method); + var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options); + if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) + { + // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。 + return new PlannedExecutionBundle( + cachedBundle.PlannedTrajectory, + cachedBundle.ShotTimeline, + CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true)); + } + var request = new TrajectoryRequest( robot: robot, program: program, - method: PlanningMethod.SelfAdaptIcsp); + method: method, + moveToStart: options.MoveToStart, + saveTrajectoryArtifacts: options.SaveTrajectory, + useCache: options.UseCache); - var plannedTrajectory = _selfAdaptIcspPlanner.Plan(request); + var plannedTrajectory = PlanByMethod(request, method); var shotTimeline = _shotTimelineBuilder.Build( plannedTrajectory, holdCycles: 0, samplePeriod: robot.ServoPeriod); - var result = CreateResult(plannedTrajectory, shotTimeline); + var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false); + var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); - return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); + if (options.UseCache) + { + _flyshotCache[cacheKey] = bundle; + } + + return bundle; + } + + /// + /// 按普通轨迹执行接口约束解析 method 参数。 + /// + /// 旧 SDK 传入的方法名。 + /// 领域层规划方法。 + 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)) + }; + } + + /// + /// 按飞拍轨迹执行接口约束解析 method 参数。 + /// + /// 旧 SDK 传入的方法名。 + /// 领域层规划方法。 + 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)) + }; + } + + /// + /// 按领域枚举分派到当前已经落地的规划器。 + /// + /// 规划请求。 + /// 规划方法。 + /// 规划轨迹。 + private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method) + { + return method switch + { + PlanningMethod.Icsp => _icspPlanner.Plan(request), + PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request), + PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"), + _ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。") + }; + } + + /// + /// 归一化旧 SDK 的 method 字符串。 + /// + /// 原始方法名。 + /// 小写短横线方法名。 + private static string NormalizeMethod(string method) + { + if (string.IsNullOrWhiteSpace(method)) + { + return "icsp"; + } + + return method.Trim().ToLowerInvariant(); + } + + /// + /// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。 + /// + /// 机器人配置。 + /// 上传轨迹。 + /// 执行参数。 + /// 缓存键。 + private static string CreateFlyshotCacheKey( + RobotProfile robot, + ControllerClientCompatUploadedTrajectory uploaded, + FlyshotExecutionOptions options) + { + var hash = new HashCode(); + hash.Add(robot.Name, StringComparer.Ordinal); + hash.Add(uploaded.Name, StringComparer.Ordinal); + hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal); + hash.Add(options.MoveToStart); + hash.Add(options.SaveTrajectory); + + 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"); } /// @@ -108,7 +250,7 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 规划后的轨迹。 /// 触发时间轴。 /// 运行时执行结果描述。 - private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline) + private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache) { return new TrajectoryResult( programName: plannedTrajectory.OriginalProgram.Name, @@ -119,7 +261,7 @@ public sealed class ControllerClientTrajectoryOrchestrator triggerTimeline: shotTimeline.TriggerTimeline, artifacts: Array.Empty(), failureReason: null, - usedCache: false, + usedCache: usedCache, originalWaypointCount: plannedTrajectory.OriginalWaypointCount, plannedWaypointCount: plannedTrajectory.PlannedWaypointCount); } diff --git a/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs new file mode 100644 index 0000000..14b4f80 --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/FlyshotExecutionOptions.cs @@ -0,0 +1,46 @@ +namespace Flyshot.ControllerClientCompat; + +/// +/// 表示飞拍轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteFlyShotTraj`。 +/// +public sealed class FlyshotExecutionOptions +{ + /// + /// 初始化飞拍轨迹执行参数。 + /// + /// 执行前是否自动移动到轨迹起点。 + /// 轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。 + /// 是否保存轨迹信息。 + /// 是否优先复用已规划轨迹缓存。 + public FlyshotExecutionOptions( + bool moveToStart = true, + string method = "icsp", + bool saveTrajectory = true, + bool useCache = true) + { + MoveToStart = moveToStart; + Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method; + SaveTrajectory = saveTrajectory; + UseCache = useCache; + } + + /// + /// 获取执行前是否自动移动到轨迹起点。 + /// + public bool MoveToStart { get; } + + /// + /// 获取轨迹生成方法。 + /// + public string Method { get; } + + /// + /// 获取是否保存轨迹信息。 + /// + public bool SaveTrajectory { get; } + + /// + /// 获取是否优先复用已规划轨迹缓存。 + /// + public bool UseCache { get; } +} diff --git a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs index 02f7175..0e8573e 100644 --- a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs @@ -1,3 +1,5 @@ +using Flyshot.Core.Domain; + namespace Flyshot.ControllerClientCompat; /// @@ -22,12 +24,38 @@ public interface IControllerClientCompatService /// 客户端传入的服务端端口。 void ConnectServer(string serverIp, int port); + /// + /// 获取兼容服务端版本号。 + /// + /// 服务端版本号。 + string GetServerVersion(); + + /// + /// 获取兼容客户端版本号。 + /// + /// 客户端版本号。 + string GetClientVersion(); + /// /// 根据旧客户端使用的机器人名称完成机器人初始化。 /// /// 机器人名称。 void SetUpRobot(string robotName); + /// + /// 根据旧客户端传入的环境文件完成机器人初始化。 + /// + /// 环境文件路径。 + void SetUpRobotFromEnv(string envFile); + + /// + /// 设置是否显示 TCP 坐标轴。 + /// + /// 是否显示 TCP。 + /// 坐标轴长度。 + /// 坐标轴线宽。 + void SetShowTcp(bool isShow, double axisLength, int axisSize); + /// /// 记录当前激活的控制器类型。 /// @@ -61,6 +89,12 @@ public interface IControllerClientCompatService /// void StopMove(); + /// + /// 读取当前控制器运行时状态快照。 + /// + /// 控制器运行时状态快照。 + ControllerStateSnapshot GetControllerSnapshot(); + /// /// 获取当前速度倍率。 /// @@ -89,6 +123,14 @@ public interface IControllerClientCompatService /// 缓存中的 IO 值。 bool GetIo(int port, string ioType); + /// + /// 按给定位姿和 seed 计算最近 IK。 + /// + /// 目标位姿数组。 + /// IK seed 关节数组。 + /// IK 结果关节数组。 + IReadOnlyList GetNearestIk(IReadOnlyList pose, IReadOnlyList seed); + /// /// 设置当前 TCP 三维坐标。 /// @@ -119,7 +161,8 @@ public interface IControllerClientCompatService /// 执行普通轨迹。 /// /// 轨迹路点集合。 - void ExecuteTrajectory(IReadOnlyList> waypoints); + /// 执行参数。 + void ExecuteTrajectory(IReadOnlyList> waypoints, TrajectoryExecutionOptions? options = null); /// /// 读取当前末端位姿快照。 @@ -143,7 +186,25 @@ public interface IControllerClientCompatService /// 执行指定名称的飞拍轨迹。 /// /// 轨迹名称。 - void ExecuteTrajectoryByName(string name); + /// 飞拍执行参数。 + void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null); + + /// + /// 保存指定飞拍轨迹的轨迹信息。 + /// + /// 轨迹名称。 + /// 轨迹生成方法。 + void SaveTrajectoryInfo(string name, string method = "icsp"); + + /// + /// 检查指定飞拍轨迹是否可执行。 + /// + /// 输出规划轨迹总时长。 + /// 轨迹名称。 + /// 轨迹生成方法。 + /// 是否保存轨迹信息。 + /// 轨迹是否有效。 + bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false); /// /// 删除指定名称的飞拍轨迹。 diff --git a/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs b/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs new file mode 100644 index 0000000..eb02bcd --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/TrajectoryExecutionOptions.cs @@ -0,0 +1,28 @@ +namespace Flyshot.ControllerClientCompat; + +/// +/// 表示普通轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteTrajectory`。 +/// +public sealed class TrajectoryExecutionOptions +{ + /// + /// 初始化普通轨迹执行参数。 + /// + /// 轨迹生成方法，支持 `icsp` 或 `doubles`。 + /// 是否保存轨迹信息。 + public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false) + { + Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method; + SaveTrajectory = saveTrajectory; + } + + /// + /// 获取轨迹生成方法。 + /// + public string Method { get; } + + /// + /// 获取是否保存轨迹信息。 + /// + public bool SaveTrajectory { get; } +} diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index ab22653..782331b 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -1,15 +1,21 @@ using Flyshot.Core.Domain; using Flyshot.Runtime.Common; +using Flyshot.Runtime.Fanuc.Protocol; namespace Flyshot.Runtime.Fanuc; /// -/// 提供第一阶段 FANUC 控制器运行时骨架，集中保存连接、使能、IO 和运动结果状态。 +/// FANUC 控制器运行时，将上层兼容层指令转换为三条真实 Socket 通道的交互。 +/// 仿真模式下仍保持内存桩行为，便于离线测试与回退。 /// -public sealed class FanucControllerRuntime : IControllerRuntime +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 = new(); + private readonly FanucStateClient _stateClient = new(); + private readonly FanucJ519Client _j519Client = new(); + private RobotProfile? _robot; private string? _robotName; private bool? _activeControllerIsSimulation; @@ -21,6 +27,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime private double[] _tcp = [0.0, 0.0, 0.0]; private double[] _jointPositions = Array.Empty(); private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]; + private bool _disposed; /// public void ResetRobot(RobotProfile robot, string robotName) @@ -33,7 +40,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { - // 重新初始化机器人时清空底层控制器状态，匹配旧 ControllerClient 的初始化顺序。 + DisconnectClients(); _robot = robot; _robotName = robotName; _activeControllerIsSimulation = null; @@ -55,6 +62,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + DisconnectClients(); _activeControllerIsSimulation = sim; _connectedRobotIp = null; _isEnabled = false; @@ -73,6 +81,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureActiveControllerSelected(); + if (_activeControllerIsSimulation!.Value) + { + _connectedRobotIp = robotIp; + _isEnabled = false; + _isInMotion = false; + return; + } + + // 真机模式：顺序建立三条通道 — 状态 → 命令 → 运动。 + _stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult(); + _commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult(); + _j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult(); + _connectedRobotIp = robotIp; _isEnabled = false; _isInMotion = false; @@ -85,6 +106,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + DisconnectClients(); _connectedRobotIp = null; _isEnabled = false; _isInMotion = false; @@ -103,6 +125,20 @@ public sealed class FanucControllerRuntime : IControllerRuntime { EnsureConnected(); _bufferSize = bufferSize; + + if (IsSimulationMode) + { + _isEnabled = true; + return; + } + + // 真机模式：走完整 RVBUSTSM 启动序列（与抓包一致）。 + _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); + _commandClient.ResetRobotAsync().GetAwaiter().GetResult(); + _commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult(); + _commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); + + _j519Client.StartMotion(); _isEnabled = true; } } @@ -113,6 +149,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + if (!IsSimulationMode) + { + _j519Client.StopMotionAsync().GetAwaiter().GetResult(); + _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); + } + _isEnabled = false; _isInMotion = false; } @@ -124,6 +166,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + if (!IsSimulationMode) + { + _j519Client.StopMotionAsync().GetAwaiter().GetResult(); + } + _isInMotion = false; } } @@ -213,6 +260,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + if (!IsSimulationMode) + { + var frame = _stateClient.GetLatestFrame(); + if (frame?.JointOrExtensionValues.Count >= _jointPositions.Length) + { + return frame.JointOrExtensionValues.Take(_jointPositions.Length).Select(v => (double)v).ToArray(); + } + } + return _jointPositions.ToArray(); } } @@ -223,6 +279,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime lock (_stateLock) { EnsureRobotSetup(); + if (!IsSimulationMode) + { + var frame = _stateClient.GetLatestFrame(); + if (frame?.Pose.Count >= 6) + { + return frame.Pose.Take(6).Select(v => (double)v).ToArray(); + } + } + return _pose.ToArray(); } } @@ -232,14 +297,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime { lock (_stateLock) { + var jointPositions = _jointPositions; + var cartesianPose = _pose; + var isInMotion = _isInMotion; + + if (!IsSimulationMode) + { + var frame = _stateClient.GetLatestFrame(); + if (frame is not null) + { + if (frame.JointOrExtensionValues.Count >= jointPositions.Length) + { + jointPositions = frame.JointOrExtensionValues.Take(jointPositions.Length).Select(v => (double)v).ToArray(); + } + + if (frame.Pose.Count >= 6) + { + cartesianPose = frame.Pose.Take(6).Select(v => (double)v).ToArray(); + } + } + + var j519Response = _j519Client.GetLatestResponse(); + if (j519Response is not null) + { + isInMotion = j519Response.RobotInMotion; + } + } + return new ControllerStateSnapshot( capturedAt: DateTimeOffset.UtcNow, connectionState: ResolveConnectionState(), isEnabled: _isEnabled, - isInMotion: _isInMotion, + isInMotion: isInMotion, speedRatio: _speedRatio, - jointPositions: _jointPositions, - cartesianPose: _pose, + jointPositions: jointPositions, + cartesianPose: cartesianPose, activeAlarms: Array.Empty()); } } @@ -256,18 +348,68 @@ public sealed class FanucControllerRuntime : IControllerRuntime EnsureValidTrajectory(result); EnsureJointCount(finalJointPositions.Count); - // 第一阶段没有真机 Socket 流，先把执行结果收敛到统一运行时状态。 + if (!IsSimulationMode) + { + // 真机模式：通过 J519 发送最终关节目标。 + // TODO: 后续接入稠密路点流，当前先发送单点收敛。 + var command = new FanucJ519Command( + sequence: 0, + targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); + _j519Client.UpdateCommand(command); + } + _isInMotion = true; _jointPositions = finalJointPositions.ToArray(); _isInMotion = false; } } + /// + /// 释放运行时持有的所有 Socket 客户端。 + /// + public void Dispose() + { + if (_disposed) + { + return; + } + + _disposed = true; + DisconnectClients(); + _commandClient.Dispose(); + _stateClient.Dispose(); + _j519Client.Dispose(); + } + + /// + /// 判断当前是否处于仿真模式；若尚未选择控制器则抛出异常。 + /// + private bool IsSimulationMode + { + get + { + if (_activeControllerIsSimulation is null) + { + throw new InvalidOperationException("Active controller has not been selected."); + } + + return _activeControllerIsSimulation.Value; + } + } + + /// + /// 断开所有真实 Socket 通道，不影响内存状态。 + /// + private void DisconnectClients() + { + _j519Client.Disconnect(); + _commandClient.Disconnect(); + _stateClient.Disconnect(); + } + /// /// 归一化 IO 类型字符串，避免调用方大小写差异影响缓存键。 /// - /// 调用方传入的 IO 类型。 - /// 标准化后的 IO 类型。 private static string NormalizeIoType(string ioType) { if (string.IsNullOrWhiteSpace(ioType)) @@ -281,7 +423,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime /// /// 校验轨迹规划结果可执行。 /// - /// 规划结果。 private static void EnsureValidTrajectory(TrajectoryResult result) { if (!result.IsValid) @@ -293,7 +434,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime /// /// 根据当前内部状态生成连接状态标签。 /// - /// 面向监控和测试的连接状态。 private string ResolveConnectionState() { if (_robot is null) @@ -307,7 +447,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime /// /// 校验给定关节数组长度与当前机器人自由度一致。 /// - /// 调用方传入的关节数。 private void EnsureJointCount(int jointCount) { var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup."); diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs new file mode 100644 index 0000000..d4e0523 --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs @@ -0,0 +1,250 @@ +using System.Net.Sockets; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// FANUC TCP 10012 命令通道客户端，提供 Req/Res 同步命令下发能力。 +/// +public sealed class FanucCommandClient : IDisposable +{ + private readonly SemaphoreSlim _sendLock = new(1, 1); + private TcpClient? _tcpClient; + private NetworkStream? _stream; + private bool _disposed; + + /// + /// 获取当前是否已建立连接。 + /// + public bool IsConnected => _tcpClient?.Connected ?? false; + + /// + /// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。 + /// + /// 控制柜 IP 地址。 + /// 命令通道端口，默认 10012。 + /// 取消令牌。 + public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default) + { + ObjectDisposedException.ThrowIf(_disposed, this); + + if (string.IsNullOrWhiteSpace(ip)) + { + throw new ArgumentException("IP 不能为空。", nameof(ip)); + } + + if (_tcpClient is not null) + { + throw new InvalidOperationException("命令通道已经连接，请先 Disconnect。"); + } + + _tcpClient = new TcpClient { NoDelay = true }; + await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false); + _stream = _tcpClient.GetStream(); + } + + /// + /// 断开命令通道并释放资源。 + /// + public void Disconnect() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + _stream?.Dispose(); + _stream = null; + _tcpClient?.Dispose(); + _tcpClient = null; + } + + /// + /// 发送通用命令并等待响应。 + /// + /// 命令消息号。 + /// 命令业务体。 + /// 取消令牌。 + /// 原始响应帧。 + public async Task SendCommandAsync(uint messageId, ReadOnlyMemory 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(); + } + } + + /// + /// 发送携带程序名的命令并等待响应。 + /// + /// 命令消息号。 + /// 程序名。 + /// 取消令牌。 + /// 结果响应。 + public async Task SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default) + { + var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName); + var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + return FanucCommandProtocol.ParseResultResponse(response); + } + + /// + /// 停止指定程序。 + /// + /// 程序名。 + /// 取消令牌。 + /// 结果响应。 + public Task StopProgramAsync(string programName, CancellationToken cancellationToken = default) + { + return SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken); + } + + /// + /// 复位控制器。 + /// + /// 取消令牌。 + /// 结果响应。 + public async Task ResetRobotAsync(CancellationToken cancellationToken = default) + { + var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot); + var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + return FanucCommandProtocol.ParseResultResponse(response); + } + + /// + /// 查询指定程序状态。 + /// + /// 程序名。 + /// 取消令牌。 + /// 程序状态响应。 + public async Task GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default) + { + var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName); + var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + return FanucCommandProtocol.ParseProgramStatusResponse(response); + } + + /// + /// 启动指定程序。 + /// + /// 程序名。 + /// 取消令牌。 + /// 结果响应。 + public Task StartProgramAsync(string programName, CancellationToken cancellationToken = default) + { + return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken); + } + + /// + /// 释放客户端资源。 + /// + public void Dispose() + { + if (_disposed) + { + return; + } + + _disposed = true; + _stream?.Dispose(); + _stream = null; + _tcpClient?.Dispose(); + _tcpClient = null; + _sendLock.Dispose(); + } + + /// + /// 直接发送已封装的帧并读取响应。 + /// + private async Task SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken) + { + if (_stream is null) + { + throw new InvalidOperationException("命令通道未连接。"); + } + + await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false); + try + { + await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false); + return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false); + } + finally + { + _sendLock.Release(); + } + } + + /// + /// 从流中读取一条完整的 doz/zod 响应帧。 + /// + private async Task 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; + } + + /// + /// 从流中精确读取指定长度的字节。 + /// + private async Task ReadExactAsync(Memory 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; + } + } +} diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs new file mode 100644 index 0000000..82fc1f7 --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs @@ -0,0 +1,272 @@ +using System.Buffers.Binary; +using System.Text; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。 +/// +public static class FanucCommandMessageIds +{ + /// + /// 获取控制器程序状态的消息号。 + /// + public const uint GetProgramStatus = 0x2003; + + /// + /// 复位控制器的消息号。 + /// + public const uint ResetRobot = 0x2100; + + /// + /// 启动控制器程序的消息号。 + /// + public const uint StartProgram = 0x2102; + + /// + /// 停止控制器程序的消息号。 + /// + public const uint StopProgram = 0x2103; + + /// + /// 读取控制器 TCP 的消息号。 + /// + public const uint GetTcp = 0x2200; + + /// + /// 设置控制器 TCP 的消息号。 + /// + public const uint SetTcp = 0x2201; + + /// + /// 读取控制器速度倍率的消息号。 + /// + public const uint GetSpeedRatio = 0x2206; + + /// + /// 设置控制器速度倍率的消息号。 + /// + public const uint SetSpeedRatio = 0x2207; + + /// + /// 读取控制器 IO 的消息号。 + /// + public const uint GetIo = 0x2208; + + /// + /// 设置控制器 IO 的消息号。 + /// + public const uint SetIo = 0x2209; +} + +/// +/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。 +/// +public sealed class FanucCommandResultResponse +{ + /// + /// 初始化命令结果响应。 + /// + /// 响应对应的消息号。 + /// 控制器返回的结果码。 + public FanucCommandResultResponse(uint messageId, uint resultCode) + { + MessageId = messageId; + ResultCode = resultCode; + } + + /// + /// 获取响应对应的消息号。 + /// + public uint MessageId { get; } + + /// + /// 获取控制器返回的结果码。 + /// + public uint ResultCode { get; } + + /// + /// 获取当前响应是否表示成功。 + /// + public bool IsSuccess => ResultCode == 0; +} + +/// +/// 表示 FANUC TCP 10012 程序状态响应。 +/// +public sealed class FanucProgramStatusResponse +{ + /// + /// 初始化程序状态响应。 + /// + /// 响应对应的消息号。 + /// 控制器返回的结果码。 + /// 控制器程序状态。 + public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus) + { + MessageId = messageId; + ResultCode = resultCode; + ProgramStatus = programStatus; + } + + /// + /// 获取响应对应的消息号。 + /// + public uint MessageId { get; } + + /// + /// 获取控制器返回的结果码。 + /// + public uint ResultCode { get; } + + /// + /// 获取控制器程序状态值。 + /// + public uint ProgramStatus { get; } + + /// + /// 获取当前响应是否表示成功。 + /// + public bool IsSuccess => ResultCode == 0; +} + +/// +/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。 +/// +public static class FanucCommandProtocol +{ + /// + /// 将无业务体命令封装为 TCP 10012 二进制帧。 + /// + /// 命令消息号。 + /// 可直接写入命令通道 Socket 的完整帧。 + public static byte[] PackEmptyCommand(uint messageId) + { + return PackFrame(messageId, ReadOnlySpan.Empty); + } + + /// + /// 将程序名命令封装为 TCP 10012 二进制帧。 + /// + /// 命令消息号。 + /// 控制器程序名。 + /// 可直接写入命令通道 Socket 的完整帧。 + 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); + } + + /// + /// 解析只携带结果码的 TCP 10012 响应帧。 + /// + /// 完整响应帧。 + /// 命令结果响应。 + public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan 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)])); + } + + /// + /// 解析 GetProgStatus 的 TCP 10012 响应帧。 + /// + /// 完整响应帧。 + /// 程序状态响应。 + public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan 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); + } + + /// + /// 按 FANUC 命令通道 framing 规则封装完整帧。 + /// + /// 命令消息号。 + /// 业务体。 + /// 完整命令帧。 + internal static byte[] PackFrame(uint messageId, ReadOnlySpan 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; + } + + /// + /// 校验完整帧并读取消息号。 + /// + /// 完整响应帧。 + /// 响应消息号。 + private static uint ValidateAndReadMessageId(ReadOnlySpan 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))); + } + + /// + /// 获取完整帧中的业务体切片。 + /// + /// 完整响应帧。 + /// 业务体切片。 + private static ReadOnlySpan GetBody(ReadOnlySpan frame) + { + return frame.Slice(11, frame.Length - 14); + } +} diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs new file mode 100644 index 0000000..81da460 --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs @@ -0,0 +1,297 @@ +using System.Net.Sockets; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供周期命令发送与响应接收能力。 +/// +public sealed class FanucJ519Client : IDisposable +{ + private readonly object _commandLock = new(); + private readonly object _responseLock = new(); + private UdpClient? _udpClient; + private CancellationTokenSource? _cts; + private Task? _sendTask; + private Task? _receiveTask; + private FanucJ519Command? _currentCommand; + private FanucJ519Response? _latestResponse; + private bool _disposed; + + /// + /// 获取当前是否已创建 UDP 套接字。 + /// + public bool IsConnected => _udpClient is not null; + + /// + /// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。 + /// + /// 控制柜 IP 地址。 + /// 运动通道端口，默认 60015。 + /// 取消令牌。 + public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default) + { + ObjectDisposedException.ThrowIf(_disposed, this); + + if (string.IsNullOrWhiteSpace(ip)) + { + throw new ArgumentException("IP 不能为空。", nameof(ip)); + } + + if (_udpClient is not null) + { + throw new InvalidOperationException("J519 通道已经连接，请先 Disconnect。"); + } + + _udpClient = new UdpClient(); + _udpClient.Connect(ip, port); + + // 发送初始化包。 + await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false); + + _cts = new CancellationTokenSource(); + _receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token); + } + + /// + /// 启动约 8ms 周期的 J519 命令发送循环。 + /// + public void StartMotion() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + if (_udpClient is null) + { + throw new InvalidOperationException("J519 通道未连接。"); + } + + if (_sendTask is not null) + { + return; // 已在运行。 + } + + _sendTask = Task.Run(() => SendLoopAsync(_cts!.Token), _cts!.Token); + } + + /// + /// 发送结束包并停止 J519 命令发送循环。 + /// + public async Task StopMotionAsync(CancellationToken cancellationToken = default) + { + ObjectDisposedException.ThrowIf(_disposed, this); + + if (_udpClient is null) + { + return; + } + + _cts?.Cancel(); + + if (_sendTask is not null) + { + try + { + await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false); + } + catch (TimeoutException) + { + // 发送循环可能未能在 1 秒内结束，继续执行后续清理。 + } + catch (OperationCanceledException) + { + // 正常取消。 + } + + _sendTask = null; + } + + // 发送结束包通知控制器停止伺服流。 + await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false); + } + + /// + /// 原子更新下一周期要发送的 J519 命令。 + /// + /// 新的 J519 命令。 + public void UpdateCommand(FanucJ519Command command) + { + ArgumentNullException.ThrowIfNull(command); + ObjectDisposedException.ThrowIf(_disposed, this); + + lock (_commandLock) + { + _currentCommand = command; + } + } + + /// + /// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。 + /// + /// 最新 J519 响应或 null。 + public FanucJ519Response? GetLatestResponse() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + lock (_responseLock) + { + return _latestResponse; + } + } + + /// + /// 断开 J519 通道并释放资源。 + /// + public void Disconnect() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + _cts?.Cancel(); + + try + { + _sendTask?.Wait(TimeSpan.FromSeconds(1)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + _sendTask?.Dispose(); + _sendTask = null; + + try + { + _receiveTask?.Wait(TimeSpan.FromSeconds(1)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + _receiveTask?.Dispose(); + _receiveTask = null; + + _cts?.Dispose(); + _cts = null; + + _udpClient?.Dispose(); + _udpClient = null; + + lock (_commandLock) + { + _currentCommand = null; + } + + lock (_responseLock) + { + _latestResponse = null; + } + } + + /// + /// 释放客户端资源。 + /// + public void Dispose() + { + if (_disposed) + { + return; + } + + _disposed = true; + _cts?.Cancel(); + + try + { + _sendTask?.Wait(TimeSpan.FromSeconds(1)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + try + { + _receiveTask?.Wait(TimeSpan.FromSeconds(1)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + _sendTask?.Dispose(); + _receiveTask?.Dispose(); + _cts?.Dispose(); + _udpClient?.Dispose(); + } + + /// + /// 后台发送循环：约 8ms 周期发送当前命令。 + /// + private async Task SendLoopAsync(CancellationToken cancellationToken) + { + if (_udpClient is null) + { + return; + } + + // 使用 8ms 周期近似 125Hz 伺服频率。 + using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(8)); + + try + { + while (await timer.WaitForNextTickAsync(cancellationToken).ConfigureAwait(false)) + { + FanucJ519Command? command; + lock (_commandLock) + { + command = _currentCommand; + } + + if (command is not null) + { + var packet = FanucJ519Protocol.PackCommandPacket(command); + await _udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); + } + } + } + catch (OperationCanceledException) + { + // 正常取消，退出循环。 + } + } + + /// + /// 后台接收循环：持续接收 132B 响应并解析。 + /// + private async Task ReceiveLoopAsync(CancellationToken cancellationToken) + { + if (_udpClient is null) + { + return; + } + + try + { + while (!cancellationToken.IsCancellationRequested) + { + var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false); + if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength) + { + var response = FanucJ519Protocol.ParseResponse(result.Buffer); + lock (_responseLock) + { + _latestResponse = response; + } + } + } + } + catch (OperationCanceledException) + { + // 正常取消，退出循环。 + } + catch (ObjectDisposedException) + { + // UDP 客户端已释放，退出循环。 + } + } +} diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs new file mode 100644 index 0000000..d84409e --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs @@ -0,0 +1,386 @@ +using System.Buffers.Binary; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。 +/// +public sealed class FanucJ519Command +{ + private readonly double[] _targetJoints; + + /// + /// 初始化 J519 命令数据。 + /// + /// 命令序号。 + /// 目标关节或扩展轴数据，最多 9 个槽位。 + /// 是否为最后一帧数据。 + /// 读取 IO 类型。 + /// 读取 IO 起始索引。 + /// 读取 IO 掩码。 + /// 目标数据类型。 + /// 写入 IO 类型。 + /// 写入 IO 起始索引。 + /// 写入 IO 掩码。 + /// 写入 IO 数值。 + public FanucJ519Command( + uint sequence, + IReadOnlyList 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(); + } + + /// + /// 获取命令序号。 + /// + public uint Sequence { get; } + + /// + /// 获取是否为最后一帧数据。 + /// + public byte LastData { get; } + + /// + /// 获取读取 IO 类型。 + /// + public byte ReadIoType { get; } + + /// + /// 获取读取 IO 起始索引。 + /// + public ushort ReadIoIndex { get; } + + /// + /// 获取读取 IO 掩码。 + /// + public ushort ReadIoMask { get; } + + /// + /// 获取目标数据类型。 + /// + public byte DataStyle { get; } + + /// + /// 获取写入 IO 类型。 + /// + public byte WriteIoType { get; } + + /// + /// 获取写入 IO 起始索引。 + /// + public ushort WriteIoIndex { get; } + + /// + /// 获取写入 IO 掩码。 + /// + public ushort WriteIoMask { get; } + + /// + /// 获取写入 IO 数值。 + /// + public ushort WriteIoValue { get; } + + /// + /// 获取目标关节或扩展轴数据。 + /// + public IReadOnlyList TargetJoints => _targetJoints; +} + +/// +/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。 +/// +public sealed class FanucJ519Response +{ + private readonly double[] _pose; + private readonly double[] _externalAxes; + private readonly double[] _jointDegrees; + private readonly double[] _motorCurrents; + + /// + /// 初始化 J519 响应数据。 + /// + /// 响应类型。 + /// 协议版本。 + /// 响应序号。 + /// 状态位集合。 + /// 读取 IO 类型。 + /// 读取 IO 起始索引。 + /// 读取 IO 掩码。 + /// 读取 IO 数值。 + /// 控制器时间戳。 + /// TCP 笛卡尔位姿。 + /// 扩展轴反馈。 + /// 关节角度反馈。 + /// 电机电流反馈。 + public FanucJ519Response( + uint messageType, + uint version, + uint sequence, + byte status, + byte readIoType, + ushort readIoIndex, + ushort readIoMask, + ushort readIoValue, + uint timestamp, + IEnumerable pose, + IEnumerable externalAxes, + IEnumerable jointDegrees, + IEnumerable 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)); + } + + /// + /// 获取响应类型。 + /// + public uint MessageType { get; } + + /// + /// 获取协议版本。 + /// + public uint Version { get; } + + /// + /// 获取响应序号。 + /// + public uint Sequence { get; } + + /// + /// 获取状态位集合。 + /// + public byte Status { get; } + + /// + /// 获取读取 IO 类型。 + /// + public byte ReadIoType { get; } + + /// + /// 获取读取 IO 起始索引。 + /// + public ushort ReadIoIndex { get; } + + /// + /// 获取读取 IO 掩码。 + /// + public ushort ReadIoMask { get; } + + /// + /// 获取读取 IO 数值。 + /// + public ushort ReadIoValue { get; } + + /// + /// 获取控制器时间戳。 + /// + public uint Timestamp { get; } + + /// + /// 获取 TCP 笛卡尔位姿。 + /// + public IReadOnlyList Pose => _pose; + + /// + /// 获取扩展轴反馈。 + /// + public IReadOnlyList ExternalAxes => _externalAxes; + + /// + /// 获取关节角度反馈。 + /// + public IReadOnlyList JointDegrees => _jointDegrees; + + /// + /// 获取电机电流反馈。 + /// + public IReadOnlyList MotorCurrents => _motorCurrents; + + /// + /// 获取控制器是否接受命令。 + /// + public bool AcceptsCommand => (Status & 0b0001) != 0; + + /// + /// 获取控制器是否已收到命令。 + /// + public bool ReceivedCommand => (Status & 0b0010) != 0; + + /// + /// 获取控制器系统是否就绪。 + /// + public bool SystemReady => (Status & 0b0100) != 0; + + /// + /// 获取机器人是否处于运动中。 + /// + public bool RobotInMotion => (Status & 0b1000) != 0; +} + +/// +/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。 +/// +public static class FanucJ519Protocol +{ + /// + /// J519 初始化和结束控制包长度。 + /// + public const int ControlPacketLength = 8; + + /// + /// J519 命令包长度。 + /// + public const int CommandPacketLength = 64; + + /// + /// J519 响应包长度。 + /// + public const int ResponsePacketLength = 132; + + /// + /// 封装 J519 初始化包。 + /// + /// 初始化包。 + public static byte[] PackInitPacket() + { + return PackControlPacket(0); + } + + /// + /// 封装 J519 结束包。 + /// + /// 结束包。 + public static byte[] PackEndPacket() + { + return PackControlPacket(2); + } + + /// + /// 封装 J519 64 字节命令包。 + /// + /// 命令数据。 + /// 命令包。 + 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; + } + + /// + /// 解析 J519 132 字节响应包。 + /// + /// 响应包。 + /// 响应解析结果。 + public static FanucJ519Response ParseResponse(ReadOnlySpan 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)); + } + + /// + /// 封装 J519 控制包。 + /// + /// 控制包类型。 + /// 控制包。 + 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; + } + + /// + /// 从响应包中读取固定长度 f32 数组。 + /// + /// 响应包。 + /// 数组起始偏移。 + /// 数组元素数量。 + /// 转换成 double 的数值数组。 + private static double[] ReadFloatArray(ReadOnlySpan 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; + } +} diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs new file mode 100644 index 0000000..b900e6d --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs @@ -0,0 +1,188 @@ +using System.Net.Sockets; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// FANUC TCP 10010 状态通道客户端，持续接收状态帧并缓存最新快照。 +/// +public sealed class FanucStateClient : IDisposable +{ + private readonly object _stateLock = new(); + private TcpClient? _tcpClient; + private NetworkStream? _stream; + private CancellationTokenSource? _receiveCts; + private Task? _receiveTask; + private FanucStateFrame? _latestFrame; + private bool _disposed; + + /// + /// 获取当前是否已建立连接。 + /// + public bool IsConnected => _tcpClient?.Connected ?? false; + + /// + /// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。 + /// + /// 控制柜 IP 地址。 + /// 状态通道端口，默认 10010。 + /// 取消令牌。 + 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 (_tcpClient is not null) + { + throw new InvalidOperationException("状态通道已经连接，请先 Disconnect。"); + } + + _tcpClient = new TcpClient { NoDelay = true }; + await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false); + _stream = _tcpClient.GetStream(); + + _receiveCts = new CancellationTokenSource(); + _receiveTask = Task.Run(() => ReceiveLoopAsync(_receiveCts.Token), _receiveCts.Token); + } + + /// + /// 断开状态通道并停止后台接收循环。 + /// + public void Disconnect() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + _receiveCts?.Cancel(); + + try + { + _receiveTask?.Wait(TimeSpan.FromSeconds(2)); + } + catch (AggregateException) + { + // 后台循环可能因取消而抛出 OperationCanceledException，忽略即可。 + } + + _receiveTask?.Dispose(); + _receiveTask = null; + _receiveCts?.Dispose(); + _receiveCts = null; + + _stream?.Dispose(); + _stream = null; + _tcpClient?.Dispose(); + _tcpClient = null; + + lock (_stateLock) + { + _latestFrame = null; + } + } + + /// + /// 获取最近一次解析的状态帧；若尚未收到任何帧则返回 null。 + /// + /// 最新状态帧或 null。 + public FanucStateFrame? GetLatestFrame() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + lock (_stateLock) + { + return _latestFrame; + } + } + + /// + /// 释放客户端资源。 + /// + public void Dispose() + { + if (_disposed) + { + return; + } + + _disposed = true; + _receiveCts?.Cancel(); + + try + { + _receiveTask?.Wait(TimeSpan.FromSeconds(2)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + _receiveTask?.Dispose(); + _receiveCts?.Dispose(); + _stream?.Dispose(); + _tcpClient?.Dispose(); + } + + /// + /// 后台循环：持续从流中读取固定长度状态帧并更新缓存。 + /// + private async Task ReceiveLoopAsync(CancellationToken cancellationToken) + { + if (_stream is null) + { + return; + } + + var buffer = new byte[FanucStateProtocol.StateFrameLength]; + + try + { + while (!cancellationToken.IsCancellationRequested) + { + await ReadExactAsync(buffer, cancellationToken).ConfigureAwait(false); + + var frame = FanucStateProtocol.ParseFrame(buffer); + lock (_stateLock) + { + _latestFrame = frame; + } + } + } + catch (OperationCanceledException) + { + // 正常取消，无需处理。 + } + catch (IOException) + { + // 连接断开，退出循环。 + } + catch (InvalidDataException) + { + // 解析到异常帧，退出循环由上层重连。 + } + } + + /// + /// 从流中精确读取固定长度字节。 + /// + private async Task ReadExactAsync(byte[] buffer, CancellationToken cancellationToken) + { + if (_stream is null) + { + throw new InvalidOperationException("状态通道未连接。"); + } + + var totalRead = 0; + while (totalRead < buffer.Length) + { + var read = await _stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken).ConfigureAwait(false); + if (read == 0) + { + throw new IOException("状态通道已断开，读取到 EOF。"); + } + + totalRead += read; + } + } +} diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs new file mode 100644 index 0000000..8749a0f --- /dev/null +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs @@ -0,0 +1,127 @@ +using System.Buffers.Binary; + +namespace Flyshot.Runtime.Fanuc.Protocol; + +/// +/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。 +/// +public sealed class FanucStateFrame +{ + private readonly double[] _pose; + private readonly double[] _jointOrExtensionValues; + private readonly uint[] _tailWords; + + /// + /// 初始化状态帧解析结果。 + /// + /// 状态帧消息号或序号。 + /// 控制器回传的笛卡尔位姿。 + /// 控制器回传的关节或扩展轴状态。 + /// 状态帧尾部状态槽位。 + public FanucStateFrame( + uint messageId, + IEnumerable pose, + IEnumerable jointOrExtensionValues, + IEnumerable 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)); + } + + /// + /// 获取状态帧消息号或序号。 + /// + public uint MessageId { get; } + + /// + /// 获取控制器回传的笛卡尔位姿。 + /// + public IReadOnlyList Pose => _pose; + + /// + /// 获取控制器回传的关节或扩展轴状态。 + /// + public IReadOnlyList JointOrExtensionValues => _jointOrExtensionValues; + + /// + /// 获取状态帧尾部状态槽位。 + /// + public IReadOnlyList TailWords => _tailWords; +} + +/// +/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。 +/// +public static class FanucStateProtocol +{ + /// + /// FANUC 状态通道抓包确认的完整帧长度。 + /// + public const int StateFrameLength = 90; + + /// + /// 解析 TCP 10010 状态通道中的单个完整状态帧。 + /// + /// 完整状态帧。 + /// 状态帧解析结果。 + public static FanucStateFrame ParseFrame(ReadOnlySpan 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); + } + + /// + /// 校验状态帧的长度、magic 和长度字段。 + /// + /// 完整状态帧。 + private static void ValidateFrame(ReadOnlySpan 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 状态帧长度字段与实际长度不一致。"); + } + } +} diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index bdfd178..8a884c2 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -51,6 +51,26 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 兼容旧 `GetServerVersion` 版本查询语义。 + /// + /// 服务端版本号。 + [HttpGet("/get_server_version/")] + public IActionResult GetServerVersion() + { + return Ok(new { server_version = _compatService.GetServerVersion() }); + } + + /// + /// 兼容旧 `GetClientVersion` 版本查询语义。 + /// + /// 客户端版本号。 + [HttpGet("/get_client_version/")] + public IActionResult GetClientVersion() + { + return Ok(new { client_version = _compatService.GetClientVersion() }); + } + /// /// 兼容旧 `/setup_robot/` 路由。 /// @@ -70,6 +90,49 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 兼容旧 `SetUpRobotFromEnv(env_file)` 参数形状。 + /// + /// 环境文件路径。 + /// 旧 FastAPI 层风格的状态响应。 + [HttpPost("/setup_robot_from_env/")] + public IActionResult SetupRobotFromEnv([FromQuery] string env_file) + { + try + { + _compatService.SetUpRobotFromEnv(env_file); + return Ok(new { status = "robot setup" }); + } + catch + { + return LegacyBadRequest("SetUpRobotFromEnv failed"); + } + } + + /// + /// 兼容旧 `SetShowTCP(is_show, axis_length, axis_size)` 参数形状。 + /// + /// 是否显示 TCP。 + /// 坐标轴长度。 + /// 坐标轴线宽。 + /// 旧 FastAPI 层风格的状态响应。 + [HttpPost("/set_show_tcp/")] + public IActionResult SetShowTcp( + [FromQuery] bool is_show = true, + [FromQuery] double axis_length = 0.1, + [FromQuery] int axis_size = 2) + { + try + { + _compatService.SetShowTcp(is_show, axis_length, axis_size); + return Ok(new { status = "show TCP set" }); + } + catch + { + return LegacyBadRequest("SetShowTCP failed"); + } + } + /// /// 兼容旧 `/is_setup/` 路由。 /// @@ -81,15 +144,16 @@ public sealed class LegacyHttpApiController : ControllerBase } /// - /// 兼容旧 `/enable_robot/` 路由；保持原 Python 服务固定传 `8` 的行为。 + /// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。 /// + /// 控制器执行缓冲区大小。 /// 旧 FastAPI 层风格的布尔状态响应。 [HttpGet("/enable_robot/")] - public IActionResult EnableRobot() + public IActionResult EnableRobot([FromQuery] int buffer_size = 2) { try { - _compatService.EnableRobot(8); + _compatService.EnableRobot(buffer_size); return Ok(new { enable_robot = true }); } catch @@ -116,6 +180,24 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 提供与旧客户端 StopMove 语义对应的 HTTP 端点。 + /// + /// 旧 FastAPI 层风格的状态响应。 + [HttpGet("/stop_move/")] + public IActionResult StopMove() + { + try + { + _compatService.StopMove(); + return Ok(new { status = "move stopped" }); + } + catch + { + return LegacyBadRequest("StopMove failed"); + } + } + /// /// 兼容旧 `/set_active_controller/` 路由。 /// @@ -154,6 +236,24 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 提供与旧客户端 Disconnect 语义对应的 HTTP 端点。 + /// + /// 旧 FastAPI 层风格的状态响应。 + [HttpPost("/disconnect_robot/")] + public IActionResult DisconnectRobot() + { + try + { + _compatService.Disconnect(); + return Ok(new { status = "robot disconnected" }); + } + catch + { + return LegacyBadRequest("Disconnect failed"); + } + } + /// /// 兼容旧 `/robot_info/` 路由。 /// @@ -234,6 +334,25 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 兼容旧 `/get_io/` 路由。 + /// + /// IO 端口号。 + /// IO 类型字符串。 + /// 当前 IO 值。 + [HttpGet("/get_io/")] + public IActionResult GetIo([FromQuery] int port, [FromQuery] string io_type) + { + try + { + return Ok(new { value = _compatService.GetIo(port, io_type) }); + } + catch + { + return LegacyBadRequest("GetDigitalOutput failed"); + } + } + /// /// 兼容旧 `/get_joint_position/` 路由。 /// @@ -270,6 +389,28 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。 + /// + /// IK 请求体。 + /// IK 结果。 + [HttpPost("/get_nearest_ik/")] + public IActionResult GetNearestIk([FromBody] LegacyNearestIkRequest request) + { + try + { + return Ok(new { success = true, ik = _compatService.GetNearestIk(request.pose, request.seed) }); + } + catch (NotSupportedException exception) + { + return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message }); + } + catch + { + return LegacyBadRequest("GetNearestIK failed"); + } + } + /// /// 兼容旧 `/list_flyShotTraj/` 路由。 /// @@ -292,11 +433,17 @@ public sealed class LegacyHttpApiController : ControllerBase /// 轨迹请求体。 /// 旧 FastAPI 层风格的状态响应。 [HttpPost("/execute_trajectory/")] - public IActionResult ExecuteTrajectory([FromBody] JsonElement waypoints) + public IActionResult ExecuteTrajectory( + [FromBody] JsonElement waypoints, + [FromQuery] string? method = null, + [FromQuery] bool? save_traj = null) { try { - _compatService.ExecuteTrajectory(ParseLegacyTrajectoryWaypoints(waypoints)); + var request = ParseExecuteTrajectoryRequest(waypoints, method, save_traj); + _compatService.ExecuteTrajectory( + request.Waypoints, + new TrajectoryExecutionOptions(request.Method, request.SaveTrajectory)); return Ok(new { status = "trajectory executed" }); } catch @@ -349,14 +496,20 @@ public sealed class LegacyHttpApiController : ControllerBase /// /// 兼容旧 `/execute_flyshot/` 路由。 /// - /// 包含轨迹名称的请求体。 + /// 包含轨迹名称和执行参数的请求体。 /// 旧 FastAPI 层风格的状态响应。 [HttpPost("/execute_flyshot/")] - public IActionResult ExecuteFlyshot([FromBody] LegacyNameRequest data) + public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data) { try { - _compatService.ExecuteTrajectoryByName(data.name); + _compatService.ExecuteTrajectoryByName( + data.name, + new FlyshotExecutionOptions( + moveToStart: data.move_to_start, + method: data.method, + saveTrajectory: data.save_traj, + useCache: data.use_cache)); return Ok(new { status = "FlyShot executed", success = true }); } catch (Exception exception) @@ -365,6 +518,57 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 兼容旧 `SaveTrajInfo(name, method)` 参数形状。 + /// + /// 轨迹保存请求体。 + /// 旧 FastAPI 层风格的状态响应。 + [HttpPost("/save_traj_info/")] + public IActionResult SaveTrajectoryInfo([FromBody] LegacyTrajectoryInfoRequest request) + { + try + { + _compatService.SaveTrajectoryInfo(request.name, request.method); + return Ok(new { status = "trajectory info saved", success = true }); + } + catch (NotSupportedException exception) + { + return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message }); + } + catch + { + return LegacyBadRequest("SaveTrajInfo failed"); + } + } + + /// + /// 兼容旧 `IsFlyShotTrajValid(time, name, method, save_traj)` 参数形状。 + /// + /// 轨迹有效性检查请求体。 + /// 有效性和轨迹时长。 + [HttpPost("/is_flyShotTrajValid/")] + public IActionResult IsFlyshotTrajectoryValid([FromBody] LegacyFlyshotValidationRequest request) + { + try + { + var isValid = _compatService.IsFlyshotTrajectoryValid( + out var duration, + request.name, + request.method, + request.save_traj); + + return Ok(new { success = isValid, valid = isValid, time = duration.TotalSeconds }); + } + catch (NotSupportedException exception) + { + return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message }); + } + catch + { + return LegacyBadRequest("IsFlyShotTrajValid failed"); + } + } + /// /// 兼容旧 `/set_speedRatio/` 路由。 /// @@ -420,7 +624,7 @@ public sealed class LegacyHttpApiController : ControllerBase return LegacyBadRequest("Robot not setup"); } - _compatService.SetActiveController(sim: false); + _compatService.SetActiveController(data.sim); _compatService.Connect(data.robot_ip); _compatService.EnableRobot(2); return Ok(new { message = "init_Success", returnCode = 0 }); @@ -448,6 +652,47 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 解析旧 `/execute_trajectory/` 的完整参数形状。 + /// + /// 原始 JSON 请求体。 + /// 查询字符串中的 method 覆盖值。 + /// 查询字符串中的 save_traj 覆盖值。 + /// 统一后的路点和执行参数。 + private static ( + IReadOnlyList> Waypoints, + string Method, + bool SaveTrajectory) ParseExecuteTrajectoryRequest( + JsonElement payload, + string? queryMethod, + bool? querySaveTrajectory) + { + string method = queryMethod ?? "icsp"; + bool saveTrajectory = querySaveTrajectory ?? false; + + if (payload.ValueKind == JsonValueKind.Object) + { + if (payload.TryGetProperty("method", out var methodElement) && methodElement.ValueKind == JsonValueKind.String) + { + method = methodElement.GetString() ?? method; + } + + if (payload.TryGetProperty("save_traj", out var saveTrajectoryElement)) + { + saveTrajectory = saveTrajectoryElement.GetBoolean(); + } + + if (!payload.TryGetProperty("waypoints", out var waypointElement)) + { + throw new InvalidOperationException("ExecuteTrajectory request body must include waypoints."); + } + + return (ParseLegacyTrajectoryWaypoints(waypointElement), method, saveTrajectory); + } + + return (ParseLegacyTrajectoryWaypoints(payload), method, saveTrajectory); + } + /// /// 解析旧 `/execute_trajectory/` 可能出现的两种历史请求体形状。 /// @@ -576,6 +821,90 @@ public sealed class LegacyNameRequest public string name { get; init; } = string.Empty; } +/// +/// 表示旧 `/execute_flyshot/` 路由使用的完整执行请求体。 +/// +public sealed class LegacyExecuteFlyshotRequest +{ + /// + /// 获取或设置轨迹名称。 + /// + public string name { get; init; } = string.Empty; + + /// + /// 获取或设置是否先移动到轨迹起点。 + /// + public bool move_to_start { get; init; } = true; + + /// + /// 获取或设置轨迹生成方法。 + /// + public string method { get; init; } = "icsp"; + + /// + /// 获取或设置是否保存轨迹信息。 + /// + public bool save_traj { get; init; } = true; + + /// + /// 获取或设置是否复用轨迹缓存。 + /// + public bool use_cache { get; init; } = true; +} + +/// +/// 表示旧 `SaveTrajInfo` 参数形状。 +/// +public sealed class LegacyTrajectoryInfoRequest +{ + /// + /// 获取或设置轨迹名称。 + /// + public string name { get; init; } = string.Empty; + + /// + /// 获取或设置轨迹生成方法。 + /// + public string method { get; init; } = "icsp"; +} + +/// +/// 表示旧 `IsFlyShotTrajValid` 参数形状。 +/// +public sealed class LegacyFlyshotValidationRequest +{ + /// + /// 获取或设置轨迹名称。 + /// + public string name { get; init; } = string.Empty; + + /// + /// 获取或设置轨迹生成方法。 + /// + public string method { get; init; } = "icsp"; + + /// + /// 获取或设置是否保存轨迹信息。 + /// + public bool save_traj { get; init; } = true; +} + +/// +/// 表示旧 `GetNearestIK` 参数形状。 +/// +public sealed class LegacyNearestIkRequest +{ + /// + /// 获取或设置目标位姿 `[x,y,z,qx,qy,qz,qw]`。 + /// + public List pose { get; init; } = []; + + /// + /// 获取或设置 IK seed 关节数组。 + /// + public List seed { get; init; } = []; +} + /// /// 表示旧 `/set_speedRatio/` 路由使用的速度倍率请求体。 /// @@ -611,4 +940,9 @@ public sealed class LegacyInitMpcRobotRequest /// 获取或设置机器人控制器 IP。 /// public string robot_ip { get; init; } = string.Empty; + + /// + /// 获取或设置是否使用仿真控制器；默认 false 连接真机。 + /// + public bool sim { get; init; } } diff --git a/src/Flyshot.Server.Host/Controllers/StatusController.cs b/src/Flyshot.Server.Host/Controllers/StatusController.cs new file mode 100644 index 0000000..1706d38 --- /dev/null +++ b/src/Flyshot.Server.Host/Controllers/StatusController.cs @@ -0,0 +1,385 @@ +using Flyshot.ControllerClientCompat; +using Microsoft.AspNetCore.Mvc; + +namespace Flyshot.Server.Host.Controllers; + +/// +/// 提供只读状态监控页面和控制器状态快照 API。 +/// +[ApiController] +public sealed class StatusController : ControllerBase +{ + /// + /// 浏览器端状态监控页面，页面逻辑只读取状态快照，不触发控制动作。 + /// + private const string StatusPageHtml = """ + + + + + + Flyshot Replacement 状态监控 + + + +
+
+

Flyshot Replacement 状态监控

+ +
+
+
+
+
+
连接状态
+
--
+
+
+
机器人
+
--
+
+
+
速度倍率
+
--
+
+
+
运动中
+
--
+
+
+
+
+

控制器

+
+
服务端版本
--
+
客户端版本
--
+
已初始化
--
+
已使能
--
+
采样时间
--
+
+
+
+

机器人

+
+
自由度
--
+
关节位置
--
+
TCP 位姿
--
+
已上传轨迹
--
+
+
+
+
+ + + +"""; + + private readonly IControllerClientCompatService _compatService; + + /// + /// 初始化状态监控控制器。 + /// + /// ControllerClient 兼容层服务。 + public StatusController(IControllerClientCompatService compatService) + { + _compatService = compatService ?? throw new ArgumentNullException(nameof(compatService)); + } + + /// + /// 返回浏览器可直接打开的状态监控页面。 + /// + /// HTML 状态页面。 + [HttpGet("/status")] + public ContentResult GetStatusPage() + { + return Content(StatusPageHtml, "text/html; charset=utf-8"); + } + + /// + /// 返回当前 ControllerClient 兼容层与控制器运行时状态快照。 + /// + /// 面向状态页和外部诊断的 JSON 快照。 + [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(); + + 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 + }); + } +} diff --git a/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs b/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs new file mode 100644 index 0000000..c1c0ce7 --- /dev/null +++ b/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs @@ -0,0 +1,179 @@ +using System.Net; +using System.Net.Sockets; +using Flyshot.Runtime.Fanuc.Protocol; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 FANUC TCP 10012 命令客户端的帧收发与响应解析。 +/// +public sealed class FanucCommandClientTests : IDisposable +{ + private readonly TcpListener _listener; + private readonly CancellationTokenSource _cts = new(); + + /// + /// 在随机可用端口启动本地模拟控制器。 + /// + public FanucCommandClientTests() + { + _listener = new TcpListener(IPAddress.Loopback, 0); + _listener.Start(); + } + + /// + /// 获取分配给本地模拟控制器的端口。 + /// + private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port; + + /// + /// 清理模拟控制器和取消源。 + /// + public void Dispose() + { + _cts.Cancel(); + _listener.Stop(); + _cts.Dispose(); + } + + /// + /// 验证命令客户端可以连接本地模拟控制器。 + /// + [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); + } + + /// + /// 验证 StopProgram 命令帧与抓包样本一致，并能解析成功响应。 + /// + [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); + } + + /// + /// 验证 ResetRobot 空命令帧能正确发送并解析结果响应。 + /// + [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); + } + + /// + /// 验证 GetProgramStatus 命令帧能正确发送并解析程序状态响应。 + /// + [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); + } + + /// + /// 验证 StartProgram 命令帧能正确发送并解析成功响应。 + /// + [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); + } + + /// + /// 验证在连接前调用命令会抛出 InvalidOperationException。 + /// + [Fact] + public async Task SendProgramCommandAsync_BeforeConnect_Throws() + { + using var client = new FanucCommandClient(); + await Assert.ThrowsAsync( + () => client.StopProgramAsync("RVBUSTSM", _cts.Token)); + } + + /// + /// 启动模拟控制器，接收一条请求帧并比对期望内容，然后返回预设响应。 + /// + 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); + } + + /// + /// 从流中精确读取指定长度的字节。 + /// + 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; + } + } +} diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs new file mode 100644 index 0000000..d453581 --- /dev/null +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -0,0 +1,180 @@ +using System.Buffers.Binary; +using System.Net; +using System.Net.Sockets; +using Flyshot.Runtime.Fanuc.Protocol; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。 +/// +public sealed class FanucJ519ClientTests : IDisposable +{ + private readonly UdpClient _server; + private readonly CancellationTokenSource _cts = new(); + + /// + /// 在随机可用端口启动本地 UDP 模拟控制器。 + /// + public FanucJ519ClientTests() + { + _server = new UdpClient(0); + } + + /// + /// 获取分配给本地模拟控制器的端口。 + /// + private int Port => ((IPEndPoint)_server.Client.LocalEndPoint!).Port; + + /// + /// 清理模拟控制器和取消源。 + /// + public void Dispose() + { + _cts.Cancel(); + _server.Dispose(); + _cts.Dispose(); + } + + /// + /// 验证连接时会发送初始化包。 + /// + [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); + } + + /// + /// 验证启动运动后能按周期发送命令包。 + /// + [Fact] + public async Task StartMotion_SendsPeriodicCommands() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + + // 接收并丢弃初始化包。 + var initResult = await _server.ReceiveAsync(_cts.Token); + + var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); + client.UpdateCommand(command); + client.StartMotion(); + + // 接收至少一个命令包。 + var commandResult = await _server.ReceiveAsync(_cts.Token); + Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length); + Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4))); + + await client.StopMotionAsync(_cts.Token); + } + + /// + /// 验证停止运动时会发送结束包。 + /// + [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); + } + + /// + /// 验证响应解析和最新响应缓存。 + /// + [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); + } + + /// + /// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。 + /// + [Fact] + public async Task UpdateCommand_ReplacesCurrentCommand() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + await _server.ReceiveAsync(_cts.Token); // init + + var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + + client.UpdateCommand(command1); + client.StartMotion(); + + var result1 = await _server.ReceiveAsync(_cts.Token); + Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4))); + Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6); + + client.UpdateCommand(command2); + + var result2 = await _server.ReceiveAsync(_cts.Token); + Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4))); + Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6); + + await client.StopMotionAsync(_cts.Token); + } + + /// + /// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。 + /// + [Fact] + public void StartMotion_BeforeConnect_Throws() + { + using var client = new FanucJ519Client(); + Assert.Throws(() => client.StartMotion()); + } +} diff --git a/tests/Flyshot.Core.Tests/FanucProtocolTests.cs b/tests/Flyshot.Core.Tests/FanucProtocolTests.cs new file mode 100644 index 0000000..b70f9d1 --- /dev/null +++ b/tests/Flyshot.Core.Tests/FanucProtocolTests.cs @@ -0,0 +1,116 @@ +using System.Buffers.Binary; +using Flyshot.Runtime.Fanuc.Protocol; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 FANUC 真机三条通信链路的二进制协议基础与逆向抓包样本一致。 +/// +public sealed class FanucProtocolTests +{ + /// + /// 验证 TCP 10012 程序命令封包与抓包中的 StopProg("RVBUSTSM") 完全一致。 + /// + [Fact] + public void CommandProtocol_PacksCapturedStopProgramFrame() + { + var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM"); + + Assert.Equal( + Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"), + frame); + } + + /// + /// 验证 TCP 10012 短响应和程序状态响应可以按抓包字段解析。 + /// + [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); + } + + /// + /// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。 + /// + [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); + } + + /// + /// 验证 UDP 60015 的 J519 初始化、结束和命令包字段布局。 + /// + [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))); + } + + /// + /// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。 + /// + [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); + } +} diff --git a/tests/Flyshot.Core.Tests/FanucStateClientTests.cs b/tests/Flyshot.Core.Tests/FanucStateClientTests.cs new file mode 100644 index 0000000..115839f --- /dev/null +++ b/tests/Flyshot.Core.Tests/FanucStateClientTests.cs @@ -0,0 +1,138 @@ +using System.Net; +using System.Net.Sockets; +using Flyshot.Runtime.Fanuc.Protocol; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 FANUC TCP 10010 状态通道客户端的后台接收与缓存能力。 +/// +public sealed class FanucStateClientTests : IDisposable +{ + private readonly TcpListener _listener; + private readonly CancellationTokenSource _cts = new(); + + /// + /// 在随机可用端口启动本地模拟控制器。 + /// + public FanucStateClientTests() + { + _listener = new TcpListener(IPAddress.Loopback, 0); + _listener.Start(); + } + + /// + /// 获取分配给本地模拟控制器的端口。 + /// + private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port; + + /// + /// 清理模拟控制器和取消源。 + /// + public void Dispose() + { + _cts.Cancel(); + _listener.Stop(); + _cts.Dispose(); + } + + /// + /// 验证状态客户端可以连接本地模拟控制器。 + /// + [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); + } + + /// + /// 验证后台循环能正确解析抓包样本状态帧。 + /// + [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); + } + + /// + /// 验证状态客户端在连接前调用 GetLatestFrame 返回 null。 + /// + [Fact] + public void GetLatestFrame_BeforeConnect_ReturnsNull() + { + using var client = new FanucStateClient(); + Assert.Null(client.GetLatestFrame()); + } + + /// + /// 验证 Disconnect 后最新帧被清空。 + /// + [Fact] + public async Task Disconnect_ClearsLatestFrame() + { + 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); + Assert.NotNull(client.GetLatestFrame()); + + client.Disconnect(); + Assert.Null(client.GetLatestFrame()); + await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token); + } + + /// + /// 启动模拟控制器，持续发送状态帧流。 + /// + 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) + { + // 客户端断开。 + } + } +} diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index f796115..f82519d 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -19,7 +19,7 @@ public sealed class RuntimeOrchestrationTests var runtime = new FanucControllerRuntime(); var robot = TestRobotFactory.CreateRobotProfile(); runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD"); - runtime.SetActiveController(sim: false); + runtime.SetActiveController(sim: true); runtime.Connect("192.168.10.101"); runtime.EnableRobot(bufferSize: 2); @@ -90,7 +90,7 @@ public sealed class RuntimeOrchestrationTests { var service = TestRobotFactory.CreateCompatService(); service.SetUpRobot("FANUC_LR_Mate_200iD"); - service.SetActiveController(sim: false); + service.SetActiveController(sim: true); service.Connect("192.168.10.101"); service.EnableRobot(2); diff --git a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs index c53ae7b..1baacdb 100644 --- a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs @@ -53,7 +53,7 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory Assert.True(isSetupJson.RootElement.GetProperty("is_setup").GetBoolean()); } - using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=false", content: null)) + 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); @@ -145,6 +145,24 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory 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()); + } } /// @@ -161,15 +179,19 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory addrs = new[] { new[] { 7, 8 }, - new[] { 7, 8 } + new[] { 7, 8 }, + Array.Empty(), + Array.Empty() }, name = "demo-http-flyshot", - offset_values = new[] { 0.0, 1.0 }, - shot_flags = new[] { false, true }, + 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[] { 1.1, 1.2, 1.3, 1.4, 1.5, 1.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 } } }; @@ -188,7 +210,27 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory Assert.Contains("demo-http-flyshot", names); } - using (var executeResponse = await client.PostAsJsonAsync("/execute_flyshot/", new { name = "demo-http-flyshot" })) + using (var validResponse = await client.PostAsJsonAsync("/is_flyShotTrajValid/", new + { + name = "demo-http-flyshot", + method = "icsp", + save_traj = false + })) + { + Assert.Equal(HttpStatusCode.OK, validResponse.StatusCode); + using var validJson = await ReadJsonAsync(validResponse); + Assert.True(validJson.RootElement.GetProperty("valid").GetBoolean()); + Assert.True(validJson.RootElement.GetProperty("time").GetDouble() > 0.0); + } + + using (var executeResponse = await client.PostAsJsonAsync("/execute_flyshot/", new + { + name = "demo-http-flyshot", + move_to_start = true, + method = "icsp", + save_traj = true, + use_cache = true + })) { Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode); using var executeJson = await ReadJsonAsync(executeResponse); @@ -197,6 +239,17 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory 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); @@ -215,7 +268,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory server_ip = "127.0.0.1", port = 50001, robot_name = "FANUC_LR_Mate_200iD", - robot_ip = "192.168.10.101" + robot_ip = "192.168.10.101", + sim = true }); Assert.Equal(HttpStatusCode.OK, initResponse.StatusCode); diff --git a/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs new file mode 100644 index 0000000..d577394 --- /dev/null +++ b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs @@ -0,0 +1,91 @@ +using System.Net; +using System.Net.Http.Json; +using System.Text.Json; + +namespace Flyshot.Server.IntegrationTests; + +/// +/// 验证状态监控页面和状态快照 API 能读取当前 ControllerClient 兼容层状态。 +/// +public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFixture +{ + /// + /// 验证状态页返回可由浏览器直接打开的 HTML，并引用状态快照 API。 + /// + [Fact] + public async Task GetStatusPage_ReturnsMonitoringHtml() + { + using var client = factory.CreateClient(); + + using var response = await client.GetAsync("/status"); + + Assert.Equal(HttpStatusCode.OK, response.StatusCode); + Assert.StartsWith("text/html", response.Content.Headers.ContentType?.MediaType); + + var html = await response.Content.ReadAsStringAsync(); + Assert.Contains("Flyshot Replacement 状态监控", html, StringComparison.Ordinal); + Assert.Contains("/api/status/snapshot", html, StringComparison.Ordinal); + } + + /// + /// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。 + /// + [Fact] + public async Task GetStatusSnapshot_ReturnsRuntimeStateAfterLegacyInitialization() + { + using var client = factory.CreateClient(); + await InitializeRobotAsync(client); + + using (var speedResponse = await client.PostAsJsonAsync("/set_speedRatio/", new { speed = 0.75 })) + { + Assert.Equal(HttpStatusCode.OK, speedResponse.StatusCode); + } + + using var response = await client.GetAsync("/api/status/snapshot"); + + Assert.Equal(HttpStatusCode.OK, response.StatusCode); + + await using var responseStream = await response.Content.ReadAsStreamAsync(); + using var json = await JsonDocument.ParseAsync(responseStream); + var root = json.RootElement; + var snapshot = root.GetProperty("snapshot"); + + Assert.Equal("ok", root.GetProperty("status").GetString()); + Assert.True(root.GetProperty("isSetup").GetBoolean()); + Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString()); + Assert.Equal(6, root.GetProperty("degreesOfFreedom").GetInt32()); + Assert.Empty(root.GetProperty("uploadedTrajectories").EnumerateArray()); + Assert.Equal("Connected", snapshot.GetProperty("connectionState").GetString()); + Assert.True(snapshot.GetProperty("isEnabled").GetBoolean()); + Assert.False(snapshot.GetProperty("isInMotion").GetBoolean()); + Assert.Equal(0.75, snapshot.GetProperty("speedRatio").GetDouble(), precision: 6); + Assert.Equal(6, snapshot.GetProperty("jointPositions").GetArrayLength()); + } + + /// + /// 初始化旧 HTTP 兼容链路，使状态页可以读取一个完整的已连接状态。 + /// + /// 测试 HTTP 客户端。 + 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); + } + } +}