yunxiao.zhu
0724efebed
* 划分 J519 发送循环与稠密轨迹循环职责边界， FanucJ519Client 负责 UDP 周期发送， FanucControllerRuntime 按轨迹时间更新下一帧命令 * 执行时将规划输出 rad 转为 J519 deg 目标， 并按 speed_ratio 调整 8ms 发送时间尺度 * 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion 状态位解析与启动前闭环检查 * MoveJoint 改为关节空间直线 + smoothstep 进度 的临时 PTP 稠密轨迹，按 status=15 运动窗口复现 * 新增 UTTC 2026-04-28 三份抓包 golden tests， 覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、 IO 脉冲与响应滞后 * 状态通道补充超时重连策略与退避逻辑 * TCP 10012 命令响应统一检查 result_code * 状态页扩展 J519 状态位与快照诊断信息 * 新增 docs/fanuc-field-runtime-workflow.md 现场工作流 * 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
188 lines
11 KiB
Markdown
188 lines
11 KiB
Markdown
# FANUC 真机协议 Socket 通信层实现计划
|
||
|
||
## 上下文
|
||
|
||
状态更新：本计划中的 Socket 客户端和 `FanucControllerRuntime` 改造已经落地；当前事实以 `README.md` 和 `docs/fanuc-field-runtime-workflow.md` 为准。本文保留为实现过程记录。
|
||
|
||
计划制定时 `flyshot-replacement` 项目已完成：
|
||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||
- HTTP 兼容层控制器和状态监控页
|
||
- 轨迹规划与飞拍触发编排层
|
||
|
||
2026-04-28 `Rvbust/uttc-20260428/20260428.pcap` 新增约束：
|
||
|
||
- `TCP 10010` 状态帧继续确认为固定 `90B`。
|
||
- `UDP 60015` 命令 `target[0..5]` 为关节角 `deg`，而 `JointDetialTraj.txt` 为 `rad`。
|
||
- `speed_ratio=0.7` 在本抓包中表现为 UDP 下发时间轴约 `1.427730x` 拉伸；机器人侧 `TCP 10012` 未抓到 `0x2207 SetSpeedRatio`。
|
||
- `UTTC_MS11` 的 17 个飞拍触发点与 17 个 UDP IO 脉冲一一对齐，`io_keep_cycles=2` 对应约两周期清零。
|
||
|
||
**历史缺失项（已完成）**：计划制定时 `FanucControllerRuntime` 仍是纯内存状态桩。当前实现已经改为持有 `FanucCommandClient`、`FanucStateClient` 和 `FanucJ519Client`，真机模式会建立三条通道并从状态/J519 响应读取运行状态。
|
||
|
||
## 目标
|
||
|
||
将 `FanucControllerRuntime` 从内存桩改造为具备真实 FANUC R30iB 通信能力的运行时，使 HTTP 层的每个指令真正下发到控制柜。
|
||
|
||
## 架构设计
|
||
|
||
### 分层结构
|
||
|
||
```
|
||
LegacyHttpApiController / StatusController (HTTP 适配层，保持不动)
|
||
↓ 调用同步接口
|
||
IControllerRuntime / ControllerClientCompatService (兼容层，保持不动)
|
||
↓ 调用同步接口
|
||
FanucControllerRuntime (改造：从内存桩 → 委托给三个 Socket 客户端)
|
||
↓ 内部持有并调度
|
||
FanucCommandClient (TCP 10012，Req/Res 同步命令通道)
|
||
FanucStateClient (TCP 10010，持续接收状态帧后台循环)
|
||
FanucJ519Client (UDP 60015，8ms 周期发送 + 接收响应)
|
||
↓ 使用现有编解码
|
||
FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改动)
|
||
```
|
||
|
||
### 关键设计决策
|
||
|
||
1. **接口保持同步**：`IControllerRuntime` 现有 18 个方法全为同步签名。内部 Socket I/O 采用 `Task` + `.GetAwaiter().GetResult()` 短时间阻塞，或后台线程 + 锁同步状态快照。避免一次性推翻整个兼容层。
|
||
|
||
2. **三个独立客户端**：每条物理通道一个类，各自管理连接生命周期，便于单独测试和故障定位。
|
||
|
||
3. **状态通道后台循环**：`FanucStateClient` 内部启动 `Task` 持续 `ReadAsync(90)`，解析状态帧后写入线程安全的 `ControllerStateSnapshot` 缓存。
|
||
|
||
4. **J519 周期发送器**：`FanucJ519Client` 内部用 `PeriodicTimer` 或 `Task.Delay` 实现约 8ms 周期的发送循环。命令通过线程安全的队列/缓冲区注入。
|
||
|
||
5. **RVBUSTSM 程序生命周期隐式管理**：`EnableRobot()` 时自动走 `StopProg→Reset→GetProgStatus→StartProg("RVBUSTSM")` 序列（与抓包一致）。`DisableRobot()` 时发送 `StopProg`。
|
||
|
||
6. **连接顺序**：`Connect()` 按顺序建立三条通道 — 先 TCP 10010（状态），再 TCP 10012（命令），最后 UDP 60015（运动）。
|
||
|
||
## 实现步骤
|
||
|
||
### Phase 1: TCP 10012 命令客户端
|
||
|
||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs`
|
||
|
||
职责：
|
||
- `Connect(string ip, int port = 10012)` — 建立 TcpClient 连接
|
||
- `SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body)` — 发送并等待响应
|
||
- `SendProgramCommandAsync(uint messageId, string programName)` — 封装程序名命令
|
||
- `Disconnect()` — 关闭连接
|
||
- 线程安全（单个命令通道同一时间只处理一个请求）
|
||
|
||
需要封装的具体命令方法：
|
||
- `StopProgramAsync(string name)` → `PackProgramCommand(0x2103, name)`
|
||
- `ResetRobotAsync()` → `PackEmptyCommand(0x2100)`
|
||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现；注意 2026-04-28 真实运行抓包未出现机器人侧 `0x2207`，执行链路仍必须在 UDP 发送时间尺度上应用当前速度倍率
|
||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||
|
||
**测试**：`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||
- 用 `TcpListener` 本地模拟控制器，验证帧收发与解析
|
||
|
||
### Phase 2: TCP 10010 状态客户端
|
||
|
||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs`
|
||
|
||
职责：
|
||
- `Connect(string ip, int port = 10010)` — 建立 TcpClient 连接
|
||
- 内部启动后台 `Task` 循环 `ReadAsync(FanucStateProtocol.StateFrameLength)`
|
||
- 每收到一帧调用 `FanucStateProtocol.ParseFrame()`
|
||
- 将解析结果写入线程安全的最新状态缓存
|
||
- 单帧接收超时后标记状态陈旧，不再把旧帧当作当前位姿/关节状态使用
|
||
- EOF、坏帧、Socket 异常或超时后关闭当前连接，并按退避策略自动重连 TCP 10010
|
||
- `GetLatestFrame()` — 返回最近一次解析的状态帧
|
||
- `GetStatus()` — 返回连接阶段、陈旧状态、最近异常和重连次数
|
||
- `Disconnect()` — 取消后台循环并关闭连接
|
||
|
||
**测试**：`tests/Flyshot.Core.Tests/FanucStateClientTests.cs`
|
||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析。
|
||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||
- `Rvbust/uttc-20260428/20260428.pcap` 再次确认 `10010` 状态帧固定 90B，平均间隔约 25.6ms。
|
||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留，不从 `[2,0,0,1]` 推断使能或运动状态。
|
||
|
||
### Phase 3: UDP 60015 J519 运动客户端
|
||
|
||
**新建文件**：`src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs`
|
||
|
||
职责：
|
||
- `Connect(string ip, int port = 60015)` — 创建 UdpClient
|
||
- 发送 init packet (`PackInitPacket()`)
|
||
- 内部启动发送循环（约 8ms 周期）
|
||
- `UpdateCommand(FanucJ519Command command)` — 原子更新下一周期要发送的命令
|
||
- `StartMotion()` — 启动发送循环
|
||
- `StopMotion()` — 发送 end packet，停止循环
|
||
- 接收线程：持续 `ReceiveAsync()` 解析 132B 响应，更新反馈状态
|
||
- `Disconnect()` — 清理
|
||
|
||
执行注意事项：
|
||
|
||
- 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。
|
||
- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。
|
||
- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。
|
||
- 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。
|
||
- 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。
|
||
|
||
**测试**：`tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs`
|
||
- 用本地 UDP socket 模拟控制器收发
|
||
|
||
### Phase 4: 重写 FanucControllerRuntime
|
||
|
||
**改造文件**：`src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs`
|
||
|
||
将当前内存桩替换为真实运行时：
|
||
|
||
- 持有三个客户端实例：`FanucCommandClient`、`FanucStateClient`、`FanucJ519Client`
|
||
- `Connect(robotIp)` — 顺序连接 10010 → 10012 → 60015
|
||
- `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519
|
||
- `DisableRobot()` — 停止 J519，发送 StopProg
|
||
- `Disconnect()` — 断开三条通道
|
||
- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送
|
||
- `StopMove()` — 立即停止 J519 发送循环
|
||
- `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值
|
||
- `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取
|
||
- `SetTcp()` / `SetSpeedRatio()` / `SetIo()` — 通过命令通道发送
|
||
|
||
### Phase 5: 端到端集成测试
|
||
|
||
**改造/新建测试**：
|
||
- `tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs` — 补充真实连接流程（可用本地模拟器）
|
||
- `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` — 用本地 TCP/UDP 模拟器验证完整链路
|
||
|
||
**验证命令**：
|
||
```bash
|
||
cd flyshot-replacement
|
||
dotnet build FlyshotReplacement.sln -v minimal
|
||
dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal
|
||
dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal
|
||
```
|
||
|
||
## 风险与回退策略
|
||
|
||
1. **真机连接风险**：第一版 Socket 实现可能有超时/重连问题。`FanucControllerRuntime` 保留 `_simulationMode` 路径，仿真模式下仍走内存桩。
|
||
2. **性能风险**：同步接口内部阻塞 Socket 可能影响 HTTP 并发。若实测有问题，后续将 `IControllerRuntime` 改为 async。
|
||
3. **现场验证风险**：TCP 10012 参数命令已按逆向结论实现，但仍需在真实 R30iB 控制柜上确认默认 `tcp_id=1`、IO 类型/地址和错误码语义。
|
||
|
||
## 关键文件清单
|
||
|
||
| 文件 | 动作 |
|
||
|------|------|
|
||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs` | 新建 |
|
||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs` | 新建 |
|
||
| `src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs` | 新建 |
|
||
| `src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs` | 重写 |
|
||
| `tests/Flyshot.Core.Tests/FanucCommandClientTests.cs` | 新建 |
|
||
| `tests/Flyshot.Core.Tests/FanucStateClientTests.cs` | 新建 |
|
||
| `tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs` | 新建 |
|
||
| `tests/Flyshot.Core.Tests/FanucControllerRuntimeSocketTests.cs` | 新建 |
|
||
|
||
## 下一步验证标准
|
||
|
||
- `FanucControllerRuntime` 的 `Connect()` 能成功建立三条 TCP/UDP 连接
|
||
- `EnableRobot()` 能走完 `RVBUSTSM` 启动序列
|
||
- `ExecuteTrajectory()` 能按 8ms 周期通过 J519 发送路点，并按当前 `speed_ratio` 推进原始轨迹时间
|
||
- `GetSnapshot()` 返回的值来自 TCP 10010 真实状态帧而非内存
|
||
- 现有 10 个集成测试和 25 个核心测试仍然通过
|