✨ feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码
* 扩展 ControllerClient 兼容层的执行参数和运行时编排 * 新增 /status 页面与 /api/status/snapshot 状态快照接口 * 补充 FANUC 协议、客户端和状态接口的最小验证测试 * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
@@ -14,7 +14,7 @@
|
|||||||
- 这是长期运行的无头后台服务,不是 GUI 桌面程序。
|
- 这是长期运行的无头后台服务,不是 GUI 桌面程序。
|
||||||
- 第一版仅面向当前现场组合,后续再扩展机型与控制柜适配。
|
- 第一版仅面向当前现场组合,后续再扩展机型与控制柜适配。
|
||||||
- 当前仓库内已经移除宿主中的 `50001/TCP+JSON` 监听实现;现阶段只保留 ASP.NET Core HTTP 控制器层,以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
|
- 当前仓库内已经移除宿主中的 `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 网关方向接回宿主。
|
- `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] 落地轨迹规划与飞拍触发时间轴
|
- [x] 落地轨迹规划与飞拍触发时间轴
|
||||||
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入最小 FANUC 运行时骨架
|
- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入最小 FANUC 运行时骨架
|
||||||
- [ ] 落地真实 `10010 / 10012 / 60015` FANUC 通讯
|
- [x] 落地 Web 状态页
|
||||||
- [ ] 落地 Web 状态页
|
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码
|
||||||
|
- [ ] 落地真实 `10010 / 10012 / 60015` FANUC Socket 通讯与现场联调
|
||||||
|
|||||||
@@ -131,5 +131,10 @@
|
|||||||
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务,不启动 `50001/TCP+JSON` 监听。
|
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务,不启动 `50001/TCP+JSON` 监听。
|
||||||
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`。
|
||||||
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹,通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`。
|
- `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` 仍保持旧兼容语义中的直接运动接口,但状态写入已经统一经过运行时,而不是由兼容服务自己维护关节数组。
|
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口,但状态写入已经统一经过运行时,而不是由兼容服务自己维护关节数组。
|
||||||
|
- `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状,但后端求解器 / 环境文件解析仍返回显式未实现。
|
||||||
|
|||||||
162
docs/fanuc-socket-implementation-plan.md
Normal file
162
docs/fanuc-socket-implementation-plan.md
Normal file
@@ -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<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()` — 待解析请求/响应体格式
|
||||||
|
- `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 个核心测试仍然通过
|
||||||
@@ -261,8 +261,9 @@ Expected: PASS, with existing HTTP compatibility tests still green.
|
|||||||
```markdown
|
```markdown
|
||||||
- [x] 落地最小 FANUC 运行时骨架
|
- [x] 落地最小 FANUC 运行时骨架
|
||||||
- [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime
|
- [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime
|
||||||
- [ ] 落地真实 10010 / 10012 / 60015 通讯
|
- [x] 落地 Web 状态页
|
||||||
- [ ] 落地 Web 状态页
|
- [x] 固化 10010 / 10012 / 60015 FANUC 基础协议帧编解码
|
||||||
|
- [ ] 落地真实 10010 / 10012 / 60015 Socket 通讯与现场联调
|
||||||
```
|
```
|
||||||
|
|
||||||
- [x] **Step 2: Run final build**
|
- [x] **Step 2: Run final build**
|
||||||
|
|||||||
@@ -18,6 +18,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
private string? _configuredRobotName;
|
private string? _configuredRobotName;
|
||||||
private string? _connectedServerIp;
|
private string? _connectedServerIp;
|
||||||
private int _connectedServerPort;
|
private int _connectedServerPort;
|
||||||
|
private bool _showTcp = true;
|
||||||
|
private double _showTcpAxisLength = 0.1;
|
||||||
|
private int _showTcpAxisSize = 2;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
|
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
|
||||||
@@ -79,6 +82,18 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public string GetServerVersion()
|
||||||
|
{
|
||||||
|
return ServerVersion;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public string GetClientVersion()
|
||||||
|
{
|
||||||
|
return "flyshot-replacement-controller-client-compat/0.1.0";
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void SetUpRobot(string robotName)
|
public void SetUpRobot(string robotName)
|
||||||
{
|
{
|
||||||
@@ -94,6 +109,41 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetUpRobotFromEnv(string envFile)
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(envFile))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。");
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SetShowTcp(bool isShow, double axisLength, int axisSize)
|
||||||
|
{
|
||||||
|
if (axisLength <= 0.0)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (axisSize <= 0)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。");
|
||||||
|
}
|
||||||
|
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
EnsureRobotSetup();
|
||||||
|
|
||||||
|
// 当前无 GUI 渲染层,先保存显示参数,保证旧 SDK 参数不会在 HTTP 边界丢失。
|
||||||
|
_showTcp = isShow;
|
||||||
|
_showTcpAxisLength = axisLength;
|
||||||
|
_showTcpAxisSize = axisSize;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void SetActiveController(bool sim)
|
public void SetActiveController(bool sim)
|
||||||
{
|
{
|
||||||
@@ -159,6 +209,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public ControllerStateSnapshot GetControllerSnapshot()
|
||||||
|
{
|
||||||
|
return _runtime.GetSnapshot();
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public double GetSpeedRatio()
|
public double GetSpeedRatio()
|
||||||
{
|
{
|
||||||
@@ -199,6 +255,29 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(pose);
|
||||||
|
ArgumentNullException.ThrowIfNull(seed);
|
||||||
|
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
EnsureRobotSetup();
|
||||||
|
if (pose.Count != 7)
|
||||||
|
{
|
||||||
|
throw new ArgumentException("位姿必须是 [x,y,z,qx,qy,qz,qw] 七元数组。", nameof(pose));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (seed.Count != GetDegreesOfFreedom())
|
||||||
|
{
|
||||||
|
throw new ArgumentException("seed 关节数量必须与机器人自由度一致。", nameof(seed));
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new NotSupportedException("GetNearestIK 尚未接入逆解求解器。");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void SetTcp(double x, double y, double z)
|
public void SetTcp(double x, double y, double z)
|
||||||
{
|
{
|
||||||
@@ -242,9 +321,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints)
|
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(waypoints);
|
ArgumentNullException.ThrowIfNull(waypoints);
|
||||||
|
options ??= new TrajectoryExecutionOptions();
|
||||||
if (waypoints.Count == 0)
|
if (waypoints.Count == 0)
|
||||||
{
|
{
|
||||||
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
|
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
|
||||||
@@ -255,8 +335,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
var robot = RequireActiveRobot();
|
var robot = RequireActiveRobot();
|
||||||
EnsureRuntimeEnabled();
|
EnsureRuntimeEnabled();
|
||||||
|
|
||||||
// 普通轨迹必须先通过 ICSP 规划,再把规划结果交给运行时执行。
|
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
|
||||||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints);
|
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options);
|
||||||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||||
}
|
}
|
||||||
@@ -294,8 +374,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void ExecuteTrajectoryByName(string name)
|
public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
|
||||||
{
|
{
|
||||||
|
options ??= new FlyshotExecutionOptions();
|
||||||
if (string.IsNullOrWhiteSpace(name))
|
if (string.IsNullOrWhiteSpace(name))
|
||||||
{
|
{
|
||||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||||
@@ -316,13 +397,68 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
|||||||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||||||
}
|
}
|
||||||
|
|
||||||
// 已上传飞拍轨迹必须生成 shot timeline 后再交给运行时。
|
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||||||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory);
|
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;
|
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||||||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(name))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||||
|
}
|
||||||
|
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
var robot = RequireActiveRobot();
|
||||||
|
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 当前阶段没有落地文件导出,先通过 saveTrajectory=true 走规划校验,避免静默接受非法参数。
|
||||||
|
_ = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||||
|
robot,
|
||||||
|
trajectory,
|
||||||
|
new FlyshotExecutionOptions(saveTrajectory: true, method: method));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <inheritdoc />
|
||||||
|
public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(name))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||||
|
}
|
||||||
|
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
var robot = RequireActiveRobot();
|
||||||
|
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||||||
|
}
|
||||||
|
|
||||||
|
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||||||
|
robot,
|
||||||
|
trajectory,
|
||||||
|
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory));
|
||||||
|
|
||||||
|
duration = bundle.Result.Duration;
|
||||||
|
return bundle.Result.IsValid;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void DeleteTrajectory(string name)
|
public void DeleteTrajectory(string name)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -12,6 +12,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
private readonly ICspPlanner _icspPlanner = new();
|
private readonly ICspPlanner _icspPlanner = new();
|
||||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new();
|
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new();
|
||||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||||
|
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 对普通轨迹执行 ICSP 规划。
|
/// 对普通轨迹执行 ICSP 规划。
|
||||||
@@ -21,10 +22,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||||||
RobotProfile robot,
|
RobotProfile robot,
|
||||||
IReadOnlyList<IReadOnlyList<double>> waypoints)
|
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||||||
|
TrajectoryExecutionOptions? options = null)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(robot);
|
ArgumentNullException.ThrowIfNull(robot);
|
||||||
ArgumentNullException.ThrowIfNull(waypoints);
|
ArgumentNullException.ThrowIfNull(waypoints);
|
||||||
|
options ??= new TrajectoryExecutionOptions();
|
||||||
|
|
||||||
var program = CreateProgram(
|
var program = CreateProgram(
|
||||||
name: "ordinary-trajectory",
|
name: "ordinary-trajectory",
|
||||||
@@ -33,14 +36,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||||||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||||||
|
|
||||||
|
var method = ParseOrdinaryMethod(options.Method);
|
||||||
var request = new TrajectoryRequest(
|
var request = new TrajectoryRequest(
|
||||||
robot: robot,
|
robot: robot,
|
||||||
program: program,
|
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<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||||
var result = CreateResult(plannedTrajectory, shotTimeline);
|
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||||
|
|
||||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||||
}
|
}
|
||||||
@@ -51,10 +56,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
/// <param name="robot">当前机器人配置。</param>
|
/// <param name="robot">当前机器人配置。</param>
|
||||||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||||||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||||||
public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded)
|
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||||
|
RobotProfile robot,
|
||||||
|
ControllerClientCompatUploadedTrajectory uploaded,
|
||||||
|
FlyshotExecutionOptions? options = null)
|
||||||
{
|
{
|
||||||
ArgumentNullException.ThrowIfNull(robot);
|
ArgumentNullException.ThrowIfNull(robot);
|
||||||
ArgumentNullException.ThrowIfNull(uploaded);
|
ArgumentNullException.ThrowIfNull(uploaded);
|
||||||
|
options ??= new FlyshotExecutionOptions();
|
||||||
|
|
||||||
var program = CreateProgram(
|
var program = CreateProgram(
|
||||||
name: uploaded.Name,
|
name: uploaded.Name,
|
||||||
@@ -63,19 +72,152 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
offsetValues: uploaded.OffsetValues,
|
offsetValues: uploaded.OffsetValues,
|
||||||
addressGroups: uploaded.AddressGroups);
|
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(
|
var request = new TrajectoryRequest(
|
||||||
robot: robot,
|
robot: robot,
|
||||||
program: program,
|
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(
|
var shotTimeline = _shotTimelineBuilder.Build(
|
||||||
plannedTrajectory,
|
plannedTrajectory,
|
||||||
holdCycles: 0,
|
holdCycles: 0,
|
||||||
samplePeriod: robot.ServoPeriod);
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按普通轨迹执行接口约束解析 method 参数。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||||
|
/// <returns>领域层规划方法。</returns>
|
||||||
|
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||||||
|
{
|
||||||
|
var normalized = NormalizeMethod(method);
|
||||||
|
return normalized switch
|
||||||
|
{
|
||||||
|
"icsp" => PlanningMethod.Icsp,
|
||||||
|
"doubles" => PlanningMethod.Doubles,
|
||||||
|
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||||
|
/// <returns>领域层规划方法。</returns>
|
||||||
|
private static PlanningMethod ParseFlyshotMethod(string method)
|
||||||
|
{
|
||||||
|
var normalized = NormalizeMethod(method);
|
||||||
|
return normalized switch
|
||||||
|
{
|
||||||
|
"icsp" => PlanningMethod.Icsp,
|
||||||
|
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||||||
|
"doubles" => PlanningMethod.Doubles,
|
||||||
|
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按领域枚举分派到当前已经落地的规划器。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="request">规划请求。</param>
|
||||||
|
/// <param name="method">规划方法。</param>
|
||||||
|
/// <returns>规划轨迹。</returns>
|
||||||
|
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method)
|
||||||
|
{
|
||||||
|
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, "未知轨迹规划方法。")
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 归一化旧 SDK 的 method 字符串。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="method">原始方法名。</param>
|
||||||
|
/// <returns>小写短横线方法名。</returns>
|
||||||
|
private static string NormalizeMethod(string method)
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(method))
|
||||||
|
{
|
||||||
|
return "icsp";
|
||||||
|
}
|
||||||
|
|
||||||
|
return method.Trim().ToLowerInvariant();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键,避免同名覆盖后误用旧规划结果。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="robot">机器人配置。</param>
|
||||||
|
/// <param name="uploaded">上传轨迹。</param>
|
||||||
|
/// <param name="options">执行参数。</param>
|
||||||
|
/// <returns>缓存键。</returns>
|
||||||
|
private static string CreateFlyshotCacheKey(
|
||||||
|
RobotProfile robot,
|
||||||
|
ControllerClientCompatUploadedTrajectory uploaded,
|
||||||
|
FlyshotExecutionOptions options)
|
||||||
|
{
|
||||||
|
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");
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -108,7 +250,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||||
/// <param name="shotTimeline">触发时间轴。</param>
|
/// <param name="shotTimeline">触发时间轴。</param>
|
||||||
/// <returns>运行时执行结果描述。</returns>
|
/// <returns>运行时执行结果描述。</returns>
|
||||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline)
|
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
|
||||||
{
|
{
|
||||||
return new TrajectoryResult(
|
return new TrajectoryResult(
|
||||||
programName: plannedTrajectory.OriginalProgram.Name,
|
programName: plannedTrajectory.OriginalProgram.Name,
|
||||||
@@ -119,7 +261,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
|||||||
triggerTimeline: shotTimeline.TriggerTimeline,
|
triggerTimeline: shotTimeline.TriggerTimeline,
|
||||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||||
failureReason: null,
|
failureReason: null,
|
||||||
usedCache: false,
|
usedCache: usedCache,
|
||||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount);
|
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,46 @@
|
|||||||
|
namespace Flyshot.ControllerClientCompat;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示飞拍轨迹执行接口的可选参数,字段名对齐旧 `ControllerClient::ExecuteFlyShotTraj`。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FlyshotExecutionOptions
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化飞拍轨迹执行参数。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
|
||||||
|
/// <param name="method">轨迹生成方法,支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
||||||
|
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||||
|
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
||||||
|
public FlyshotExecutionOptions(
|
||||||
|
bool moveToStart = true,
|
||||||
|
string method = "icsp",
|
||||||
|
bool saveTrajectory = true,
|
||||||
|
bool useCache = true)
|
||||||
|
{
|
||||||
|
MoveToStart = moveToStart;
|
||||||
|
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||||
|
SaveTrajectory = saveTrajectory;
|
||||||
|
UseCache = useCache;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取执行前是否自动移动到轨迹起点。
|
||||||
|
/// </summary>
|
||||||
|
public bool MoveToStart { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取轨迹生成方法。
|
||||||
|
/// </summary>
|
||||||
|
public string Method { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否保存轨迹信息。
|
||||||
|
/// </summary>
|
||||||
|
public bool SaveTrajectory { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否优先复用已规划轨迹缓存。
|
||||||
|
/// </summary>
|
||||||
|
public bool UseCache { get; }
|
||||||
|
}
|
||||||
@@ -1,3 +1,5 @@
|
|||||||
|
using Flyshot.Core.Domain;
|
||||||
|
|
||||||
namespace Flyshot.ControllerClientCompat;
|
namespace Flyshot.ControllerClientCompat;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -22,12 +24,38 @@ public interface IControllerClientCompatService
|
|||||||
/// <param name="port">客户端传入的服务端端口。</param>
|
/// <param name="port">客户端传入的服务端端口。</param>
|
||||||
void ConnectServer(string serverIp, int port);
|
void ConnectServer(string serverIp, int port);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取兼容服务端版本号。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>服务端版本号。</returns>
|
||||||
|
string GetServerVersion();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取兼容客户端版本号。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>客户端版本号。</returns>
|
||||||
|
string GetClientVersion();
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 根据旧客户端使用的机器人名称完成机器人初始化。
|
/// 根据旧客户端使用的机器人名称完成机器人初始化。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="robotName">机器人名称。</param>
|
/// <param name="robotName">机器人名称。</param>
|
||||||
void SetUpRobot(string robotName);
|
void SetUpRobot(string robotName);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 根据旧客户端传入的环境文件完成机器人初始化。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="envFile">环境文件路径。</param>
|
||||||
|
void SetUpRobotFromEnv(string envFile);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 设置是否显示 TCP 坐标轴。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="isShow">是否显示 TCP。</param>
|
||||||
|
/// <param name="axisLength">坐标轴长度。</param>
|
||||||
|
/// <param name="axisSize">坐标轴线宽。</param>
|
||||||
|
void SetShowTcp(bool isShow, double axisLength, int axisSize);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 记录当前激活的控制器类型。
|
/// 记录当前激活的控制器类型。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -61,6 +89,12 @@ public interface IControllerClientCompatService
|
|||||||
/// </summary>
|
/// </summary>
|
||||||
void StopMove();
|
void StopMove();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 读取当前控制器运行时状态快照。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>控制器运行时状态快照。</returns>
|
||||||
|
ControllerStateSnapshot GetControllerSnapshot();
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 获取当前速度倍率。
|
/// 获取当前速度倍率。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -89,6 +123,14 @@ public interface IControllerClientCompatService
|
|||||||
/// <returns>缓存中的 IO 值。</returns>
|
/// <returns>缓存中的 IO 值。</returns>
|
||||||
bool GetIo(int port, string ioType);
|
bool GetIo(int port, string ioType);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按给定位姿和 seed 计算最近 IK。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="pose">目标位姿数组。</param>
|
||||||
|
/// <param name="seed">IK seed 关节数组。</param>
|
||||||
|
/// <returns>IK 结果关节数组。</returns>
|
||||||
|
IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 设置当前 TCP 三维坐标。
|
/// 设置当前 TCP 三维坐标。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -119,7 +161,8 @@ public interface IControllerClientCompatService
|
|||||||
/// 执行普通轨迹。
|
/// 执行普通轨迹。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="waypoints">轨迹路点集合。</param>
|
/// <param name="waypoints">轨迹路点集合。</param>
|
||||||
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints);
|
/// <param name="options">执行参数。</param>
|
||||||
|
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 读取当前末端位姿快照。
|
/// 读取当前末端位姿快照。
|
||||||
@@ -143,7 +186,25 @@ public interface IControllerClientCompatService
|
|||||||
/// 执行指定名称的飞拍轨迹。
|
/// 执行指定名称的飞拍轨迹。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="name">轨迹名称。</param>
|
/// <param name="name">轨迹名称。</param>
|
||||||
void ExecuteTrajectoryByName(string name);
|
/// <param name="options">飞拍执行参数。</param>
|
||||||
|
void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 保存指定飞拍轨迹的轨迹信息。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="name">轨迹名称。</param>
|
||||||
|
/// <param name="method">轨迹生成方法。</param>
|
||||||
|
void SaveTrajectoryInfo(string name, string method = "icsp");
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 检查指定飞拍轨迹是否可执行。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="duration">输出规划轨迹总时长。</param>
|
||||||
|
/// <param name="name">轨迹名称。</param>
|
||||||
|
/// <param name="method">轨迹生成方法。</param>
|
||||||
|
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||||
|
/// <returns>轨迹是否有效。</returns>
|
||||||
|
bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false);
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 删除指定名称的飞拍轨迹。
|
/// 删除指定名称的飞拍轨迹。
|
||||||
|
|||||||
@@ -0,0 +1,28 @@
|
|||||||
|
namespace Flyshot.ControllerClientCompat;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示普通轨迹执行接口的可选参数,字段名对齐旧 `ControllerClient::ExecuteTrajectory`。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class TrajectoryExecutionOptions
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化普通轨迹执行参数。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="method">轨迹生成方法,支持 `icsp` 或 `doubles`。</param>
|
||||||
|
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||||
|
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
|
||||||
|
{
|
||||||
|
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||||
|
SaveTrajectory = saveTrajectory;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取轨迹生成方法。
|
||||||
|
/// </summary>
|
||||||
|
public string Method { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否保存轨迹信息。
|
||||||
|
/// </summary>
|
||||||
|
public bool SaveTrajectory { get; }
|
||||||
|
}
|
||||||
@@ -1,15 +1,21 @@
|
|||||||
using Flyshot.Core.Domain;
|
using Flyshot.Core.Domain;
|
||||||
using Flyshot.Runtime.Common;
|
using Flyshot.Runtime.Common;
|
||||||
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
namespace Flyshot.Runtime.Fanuc;
|
namespace Flyshot.Runtime.Fanuc;
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 提供第一阶段 FANUC 控制器运行时骨架,集中保存连接、使能、IO 和运动结果状态。
|
/// FANUC 控制器运行时,将上层兼容层指令转换为三条真实 Socket 通道的交互。
|
||||||
|
/// 仿真模式下仍保持内存桩行为,便于离线测试与回退。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public sealed class FanucControllerRuntime : IControllerRuntime
|
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||||
{
|
{
|
||||||
private readonly object _stateLock = new();
|
private readonly object _stateLock = new();
|
||||||
private readonly Dictionary<(string IoType, int Port), bool> _ioValues = 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 RobotProfile? _robot;
|
||||||
private string? _robotName;
|
private string? _robotName;
|
||||||
private bool? _activeControllerIsSimulation;
|
private bool? _activeControllerIsSimulation;
|
||||||
@@ -21,6 +27,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
private double[] _tcp = [0.0, 0.0, 0.0];
|
private double[] _tcp = [0.0, 0.0, 0.0];
|
||||||
private double[] _jointPositions = Array.Empty<double>();
|
private double[] _jointPositions = Array.Empty<double>();
|
||||||
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
|
||||||
|
private bool _disposed;
|
||||||
|
|
||||||
/// <inheritdoc />
|
/// <inheritdoc />
|
||||||
public void ResetRobot(RobotProfile robot, string robotName)
|
public void ResetRobot(RobotProfile robot, string robotName)
|
||||||
@@ -33,7 +40,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
|
|
||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
// 重新初始化机器人时清空底层控制器状态,匹配旧 ControllerClient 的初始化顺序。
|
DisconnectClients();
|
||||||
_robot = robot;
|
_robot = robot;
|
||||||
_robotName = robotName;
|
_robotName = robotName;
|
||||||
_activeControllerIsSimulation = null;
|
_activeControllerIsSimulation = null;
|
||||||
@@ -55,6 +62,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
EnsureRobotSetup();
|
||||||
|
DisconnectClients();
|
||||||
_activeControllerIsSimulation = sim;
|
_activeControllerIsSimulation = sim;
|
||||||
_connectedRobotIp = null;
|
_connectedRobotIp = null;
|
||||||
_isEnabled = false;
|
_isEnabled = false;
|
||||||
@@ -73,6 +81,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureActiveControllerSelected();
|
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;
|
_connectedRobotIp = robotIp;
|
||||||
_isEnabled = false;
|
_isEnabled = false;
|
||||||
_isInMotion = false;
|
_isInMotion = false;
|
||||||
@@ -85,6 +106,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
EnsureRobotSetup();
|
||||||
|
DisconnectClients();
|
||||||
_connectedRobotIp = null;
|
_connectedRobotIp = null;
|
||||||
_isEnabled = false;
|
_isEnabled = false;
|
||||||
_isInMotion = false;
|
_isInMotion = false;
|
||||||
@@ -103,6 +125,20 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
{
|
{
|
||||||
EnsureConnected();
|
EnsureConnected();
|
||||||
_bufferSize = bufferSize;
|
_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;
|
_isEnabled = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -113,6 +149,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
EnsureRobotSetup();
|
||||||
|
if (!IsSimulationMode)
|
||||||
|
{
|
||||||
|
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||||
|
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
|
||||||
|
}
|
||||||
|
|
||||||
_isEnabled = false;
|
_isEnabled = false;
|
||||||
_isInMotion = false;
|
_isInMotion = false;
|
||||||
}
|
}
|
||||||
@@ -124,6 +166,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
EnsureRobotSetup();
|
||||||
|
if (!IsSimulationMode)
|
||||||
|
{
|
||||||
|
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
|
||||||
|
}
|
||||||
|
|
||||||
_isInMotion = false;
|
_isInMotion = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -213,6 +260,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
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();
|
return _jointPositions.ToArray();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -223,6 +279,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
lock (_stateLock)
|
lock (_stateLock)
|
||||||
{
|
{
|
||||||
EnsureRobotSetup();
|
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();
|
return _pose.ToArray();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -232,14 +297,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
{
|
{
|
||||||
lock (_stateLock)
|
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(
|
return new ControllerStateSnapshot(
|
||||||
capturedAt: DateTimeOffset.UtcNow,
|
capturedAt: DateTimeOffset.UtcNow,
|
||||||
connectionState: ResolveConnectionState(),
|
connectionState: ResolveConnectionState(),
|
||||||
isEnabled: _isEnabled,
|
isEnabled: _isEnabled,
|
||||||
isInMotion: _isInMotion,
|
isInMotion: isInMotion,
|
||||||
speedRatio: _speedRatio,
|
speedRatio: _speedRatio,
|
||||||
jointPositions: _jointPositions,
|
jointPositions: jointPositions,
|
||||||
cartesianPose: _pose,
|
cartesianPose: cartesianPose,
|
||||||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -256,18 +348,68 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
EnsureValidTrajectory(result);
|
EnsureValidTrajectory(result);
|
||||||
EnsureJointCount(finalJointPositions.Count);
|
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;
|
_isInMotion = true;
|
||||||
_jointPositions = finalJointPositions.ToArray();
|
_jointPositions = finalJointPositions.ToArray();
|
||||||
_isInMotion = false;
|
_isInMotion = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 释放运行时持有的所有 Socket 客户端。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
if (_disposed)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_disposed = true;
|
||||||
|
DisconnectClients();
|
||||||
|
_commandClient.Dispose();
|
||||||
|
_stateClient.Dispose();
|
||||||
|
_j519Client.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 判断当前是否处于仿真模式;若尚未选择控制器则抛出异常。
|
||||||
|
/// </summary>
|
||||||
|
private bool IsSimulationMode
|
||||||
|
{
|
||||||
|
get
|
||||||
|
{
|
||||||
|
if (_activeControllerIsSimulation is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("Active controller has not been selected.");
|
||||||
|
}
|
||||||
|
|
||||||
|
return _activeControllerIsSimulation.Value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 断开所有真实 Socket 通道,不影响内存状态。
|
||||||
|
/// </summary>
|
||||||
|
private void DisconnectClients()
|
||||||
|
{
|
||||||
|
_j519Client.Disconnect();
|
||||||
|
_commandClient.Disconnect();
|
||||||
|
_stateClient.Disconnect();
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 归一化 IO 类型字符串,避免调用方大小写差异影响缓存键。
|
/// 归一化 IO 类型字符串,避免调用方大小写差异影响缓存键。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="ioType">调用方传入的 IO 类型。</param>
|
|
||||||
/// <returns>标准化后的 IO 类型。</returns>
|
|
||||||
private static string NormalizeIoType(string ioType)
|
private static string NormalizeIoType(string ioType)
|
||||||
{
|
{
|
||||||
if (string.IsNullOrWhiteSpace(ioType))
|
if (string.IsNullOrWhiteSpace(ioType))
|
||||||
@@ -281,7 +423,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 校验轨迹规划结果可执行。
|
/// 校验轨迹规划结果可执行。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="result">规划结果。</param>
|
|
||||||
private static void EnsureValidTrajectory(TrajectoryResult result)
|
private static void EnsureValidTrajectory(TrajectoryResult result)
|
||||||
{
|
{
|
||||||
if (!result.IsValid)
|
if (!result.IsValid)
|
||||||
@@ -293,7 +434,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 根据当前内部状态生成连接状态标签。
|
/// 根据当前内部状态生成连接状态标签。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <returns>面向监控和测试的连接状态。</returns>
|
|
||||||
private string ResolveConnectionState()
|
private string ResolveConnectionState()
|
||||||
{
|
{
|
||||||
if (_robot is null)
|
if (_robot is null)
|
||||||
@@ -307,7 +447,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 校验给定关节数组长度与当前机器人自由度一致。
|
/// 校验给定关节数组长度与当前机器人自由度一致。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="jointCount">调用方传入的关节数。</param>
|
|
||||||
private void EnsureJointCount(int jointCount)
|
private void EnsureJointCount(int jointCount)
|
||||||
{
|
{
|
||||||
var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
||||||
|
|||||||
250
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
250
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
@@ -0,0 +1,250 @@
|
|||||||
|
using System.Net.Sockets;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// FANUC TCP 10012 命令通道客户端,提供 Req/Res 同步命令下发能力。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucCommandClient : IDisposable
|
||||||
|
{
|
||||||
|
private readonly SemaphoreSlim _sendLock = new(1, 1);
|
||||||
|
private TcpClient? _tcpClient;
|
||||||
|
private NetworkStream? _stream;
|
||||||
|
private bool _disposed;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取当前是否已建立连接。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="ip">控制柜 IP 地址。</param>
|
||||||
|
/// <param name="port">命令通道端口,默认 10012。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (string.IsNullOrWhiteSpace(ip))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_tcpClient is not null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("命令通道已经连接,请先 Disconnect。");
|
||||||
|
}
|
||||||
|
|
||||||
|
_tcpClient = new TcpClient { NoDelay = true };
|
||||||
|
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||||
|
_stream = _tcpClient.GetStream();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 断开命令通道并释放资源。
|
||||||
|
/// </summary>
|
||||||
|
public void Disconnect()
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
_stream?.Dispose();
|
||||||
|
_stream = null;
|
||||||
|
_tcpClient?.Dispose();
|
||||||
|
_tcpClient = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 发送通用命令并等待响应。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">命令消息号。</param>
|
||||||
|
/// <param name="body">命令业务体。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>原始响应帧。</returns>
|
||||||
|
public async Task<byte[]> SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (_stream is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("命令通道未连接。");
|
||||||
|
}
|
||||||
|
|
||||||
|
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var frame = FanucCommandProtocol.PackFrame(messageId, body.Span);
|
||||||
|
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||||
|
|
||||||
|
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
_sendLock.Release();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 发送携带程序名的命令并等待响应。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">命令消息号。</param>
|
||||||
|
/// <param name="programName">程序名。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>结果响应。</returns>
|
||||||
|
public async Task<FanucCommandResultResponse> SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName);
|
||||||
|
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||||
|
return FanucCommandProtocol.ParseResultResponse(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停止指定程序。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="programName">程序名。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>结果响应。</returns>
|
||||||
|
public Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
return SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 复位控制器。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>结果响应。</returns>
|
||||||
|
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||||
|
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||||
|
return FanucCommandProtocol.ParseResultResponse(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 查询指定程序状态。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="programName">程序名。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>程序状态响应。</returns>
|
||||||
|
public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
|
||||||
|
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||||
|
return FanucCommandProtocol.ParseProgramStatusResponse(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 启动指定程序。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="programName">程序名。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
/// <returns>结果响应。</returns>
|
||||||
|
public Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 释放客户端资源。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
if (_disposed)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_disposed = true;
|
||||||
|
_stream?.Dispose();
|
||||||
|
_stream = null;
|
||||||
|
_tcpClient?.Dispose();
|
||||||
|
_tcpClient = null;
|
||||||
|
_sendLock.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 直接发送已封装的帧并读取响应。
|
||||||
|
/// </summary>
|
||||||
|
private async Task<byte[]> SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
if (_stream is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("命令通道未连接。");
|
||||||
|
}
|
||||||
|
|
||||||
|
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||||
|
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
_sendLock.Release();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从流中读取一条完整的 doz/zod 响应帧。
|
||||||
|
/// </summary>
|
||||||
|
private async Task<byte[]> ReadResponseFrameAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
if (_stream is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("命令通道未连接。");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 先读取 11 字节头:doz(3) + length(4) + msg_id(4)
|
||||||
|
var header = new byte[11];
|
||||||
|
await ReadExactAsync(header, cancellationToken).ConfigureAwait(false);
|
||||||
|
|
||||||
|
if (header[0] != (byte)'d' || header[1] != (byte)'o' || header[2] != (byte)'z')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("响应帧头 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var declaredLength = System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(header.AsSpan(3, 4));
|
||||||
|
if (declaredLength < 14)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("响应帧声明长度过短。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var remaining = (int)declaredLength - 11;
|
||||||
|
var frame = new byte[declaredLength];
|
||||||
|
header.CopyTo(frame, 0);
|
||||||
|
await ReadExactAsync(frame.AsMemory(11, remaining), cancellationToken).ConfigureAwait(false);
|
||||||
|
|
||||||
|
// 校验帧尾
|
||||||
|
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("响应帧尾 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
return frame;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从流中精确读取指定长度的字节。
|
||||||
|
/// </summary>
|
||||||
|
private async Task ReadExactAsync(Memory<byte> buffer, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
if (_stream is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("命令通道未连接。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var totalRead = 0;
|
||||||
|
while (totalRead < buffer.Length)
|
||||||
|
{
|
||||||
|
var read = await _stream.ReadAsync(buffer.Slice(totalRead), cancellationToken).ConfigureAwait(false);
|
||||||
|
if (read == 0)
|
||||||
|
{
|
||||||
|
throw new IOException("命令通道已断开,读取到 EOF。");
|
||||||
|
}
|
||||||
|
|
||||||
|
totalRead += read;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
272
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
272
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
@@ -0,0 +1,272 @@
|
|||||||
|
using System.Buffers.Binary;
|
||||||
|
using System.Text;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public static class FanucCommandMessageIds
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器程序状态的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint GetProgramStatus = 0x2003;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 复位控制器的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint ResetRobot = 0x2100;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 启动控制器程序的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint StartProgram = 0x2102;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停止控制器程序的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint StopProgram = 0x2103;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 读取控制器 TCP 的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint GetTcp = 0x2200;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 设置控制器 TCP 的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint SetTcp = 0x2201;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 读取控制器速度倍率的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint GetSpeedRatio = 0x2206;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 设置控制器速度倍率的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint SetSpeedRatio = 0x2207;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 读取控制器 IO 的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint GetIo = 0x2208;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 设置控制器 IO 的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public const uint SetIo = 0x2209;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucCommandResultResponse
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化命令结果响应。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">响应对应的消息号。</param>
|
||||||
|
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||||
|
public FanucCommandResultResponse(uint messageId, uint resultCode)
|
||||||
|
{
|
||||||
|
MessageId = messageId;
|
||||||
|
ResultCode = resultCode;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取响应对应的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public uint MessageId { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器返回的结果码。
|
||||||
|
/// </summary>
|
||||||
|
public uint ResultCode { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取当前响应是否表示成功。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsSuccess => ResultCode == 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucProgramStatusResponse
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化程序状态响应。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">响应对应的消息号。</param>
|
||||||
|
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||||
|
/// <param name="programStatus">控制器程序状态。</param>
|
||||||
|
public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
|
||||||
|
{
|
||||||
|
MessageId = messageId;
|
||||||
|
ResultCode = resultCode;
|
||||||
|
ProgramStatus = programStatus;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取响应对应的消息号。
|
||||||
|
/// </summary>
|
||||||
|
public uint MessageId { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器返回的结果码。
|
||||||
|
/// </summary>
|
||||||
|
public uint ResultCode { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器程序状态值。
|
||||||
|
/// </summary>
|
||||||
|
public uint ProgramStatus { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取当前响应是否表示成功。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsSuccess => ResultCode == 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
|
||||||
|
/// </summary>
|
||||||
|
public static class FanucCommandProtocol
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 将无业务体命令封装为 TCP 10012 二进制帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">命令消息号。</param>
|
||||||
|
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||||
|
public static byte[] PackEmptyCommand(uint messageId)
|
||||||
|
{
|
||||||
|
return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将程序名命令封装为 TCP 10012 二进制帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">命令消息号。</param>
|
||||||
|
/// <param name="programName">控制器程序名。</param>
|
||||||
|
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||||
|
public static byte[] PackProgramCommand(uint messageId, string programName)
|
||||||
|
{
|
||||||
|
if (string.IsNullOrWhiteSpace(programName))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("程序名不能为空。", nameof(programName));
|
||||||
|
}
|
||||||
|
|
||||||
|
var programNameBytes = Encoding.ASCII.GetBytes(programName);
|
||||||
|
var body = new byte[sizeof(uint) + programNameBytes.Length];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
|
||||||
|
programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
|
||||||
|
|
||||||
|
return PackFrame(messageId, body);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整响应帧。</param>
|
||||||
|
/// <returns>命令结果响应。</returns>
|
||||||
|
public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
var messageId = ValidateAndReadMessageId(frame);
|
||||||
|
var body = GetBody(frame);
|
||||||
|
if (body.Length < sizeof(uint))
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 命令响应体长度不足。");
|
||||||
|
}
|
||||||
|
|
||||||
|
return new FanucCommandResultResponse(
|
||||||
|
messageId,
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整响应帧。</param>
|
||||||
|
/// <returns>程序状态响应。</returns>
|
||||||
|
public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
var messageId = ValidateAndReadMessageId(frame);
|
||||||
|
var body = GetBody(frame);
|
||||||
|
if (body.Length < sizeof(uint) * 2)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||||||
|
}
|
||||||
|
|
||||||
|
// 抓包样本中的字段顺序为 result_code 后接 prog_status。
|
||||||
|
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||||
|
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||||
|
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按 FANUC 命令通道 framing 规则封装完整帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">命令消息号。</param>
|
||||||
|
/// <param name="body">业务体。</param>
|
||||||
|
/// <returns>完整命令帧。</returns>
|
||||||
|
internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
|
||||||
|
{
|
||||||
|
var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
|
||||||
|
var frame = new byte[frameLength];
|
||||||
|
|
||||||
|
frame[0] = (byte)'d';
|
||||||
|
frame[1] = (byte)'o';
|
||||||
|
frame[2] = (byte)'z';
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
|
||||||
|
body.CopyTo(frame.AsSpan(11));
|
||||||
|
frame[^3] = (byte)'z';
|
||||||
|
frame[^2] = (byte)'o';
|
||||||
|
frame[^1] = (byte)'d';
|
||||||
|
|
||||||
|
return frame;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 校验完整帧并读取消息号。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整响应帧。</param>
|
||||||
|
/// <returns>响应消息号。</returns>
|
||||||
|
private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
if (frame.Length < 14)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 命令帧长度不足。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||||
|
if (declaredLength != frame.Length)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
|
||||||
|
}
|
||||||
|
|
||||||
|
return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取完整帧中的业务体切片。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整响应帧。</param>
|
||||||
|
/// <returns>业务体切片。</returns>
|
||||||
|
private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
return frame.Slice(11, frame.Length - 14);
|
||||||
|
}
|
||||||
|
}
|
||||||
297
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
297
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
@@ -0,0 +1,297 @@
|
|||||||
|
using System.Net.Sockets;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端,提供周期命令发送与响应接收能力。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucJ519Client : IDisposable
|
||||||
|
{
|
||||||
|
private readonly object _commandLock = new();
|
||||||
|
private readonly object _responseLock = new();
|
||||||
|
private UdpClient? _udpClient;
|
||||||
|
private CancellationTokenSource? _cts;
|
||||||
|
private Task? _sendTask;
|
||||||
|
private Task? _receiveTask;
|
||||||
|
private FanucJ519Command? _currentCommand;
|
||||||
|
private FanucJ519Response? _latestResponse;
|
||||||
|
private bool _disposed;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取当前是否已创建 UDP 套接字。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsConnected => _udpClient is not null;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="ip">控制柜 IP 地址。</param>
|
||||||
|
/// <param name="port">运动通道端口,默认 60015。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (string.IsNullOrWhiteSpace(ip))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_udpClient is not null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("J519 通道已经连接,请先 Disconnect。");
|
||||||
|
}
|
||||||
|
|
||||||
|
_udpClient = new UdpClient();
|
||||||
|
_udpClient.Connect(ip, port);
|
||||||
|
|
||||||
|
// 发送初始化包。
|
||||||
|
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||||
|
|
||||||
|
_cts = new CancellationTokenSource();
|
||||||
|
_receiveTask = Task.Run(() => ReceiveLoopAsync(_cts.Token), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 启动约 8ms 周期的 J519 命令发送循环。
|
||||||
|
/// </summary>
|
||||||
|
public void StartMotion()
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (_udpClient is null)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("J519 通道未连接。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_sendTask is not null)
|
||||||
|
{
|
||||||
|
return; // 已在运行。
|
||||||
|
}
|
||||||
|
|
||||||
|
_sendTask = Task.Run(() => SendLoopAsync(_cts!.Token), _cts!.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 发送结束包并停止 J519 命令发送循环。
|
||||||
|
/// </summary>
|
||||||
|
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (_udpClient is null)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_cts?.Cancel();
|
||||||
|
|
||||||
|
if (_sendTask is not null)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
await _sendTask.WaitAsync(TimeSpan.FromSeconds(1), cancellationToken).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
catch (TimeoutException)
|
||||||
|
{
|
||||||
|
// 发送循环可能未能在 1 秒内结束,继续执行后续清理。
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// 正常取消。
|
||||||
|
}
|
||||||
|
|
||||||
|
_sendTask = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 发送结束包通知控制器停止伺服流。
|
||||||
|
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 原子更新下一周期要发送的 J519 命令。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="command">新的 J519 命令。</param>
|
||||||
|
public void UpdateCommand(FanucJ519Command command)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(command);
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
lock (_commandLock)
|
||||||
|
{
|
||||||
|
_currentCommand = command;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取最近一次解析的 J519 响应;若尚未收到任何响应则返回 null。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>最新 J519 响应或 null。</returns>
|
||||||
|
public FanucJ519Response? GetLatestResponse()
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
lock (_responseLock)
|
||||||
|
{
|
||||||
|
return _latestResponse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 断开 J519 通道并释放资源。
|
||||||
|
/// </summary>
|
||||||
|
public void Disconnect()
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
_cts?.Cancel();
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
|
}
|
||||||
|
catch (AggregateException)
|
||||||
|
{
|
||||||
|
// 忽略取消异常。
|
||||||
|
}
|
||||||
|
|
||||||
|
_sendTask?.Dispose();
|
||||||
|
_sendTask = null;
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
|
}
|
||||||
|
catch (AggregateException)
|
||||||
|
{
|
||||||
|
// 忽略取消异常。
|
||||||
|
}
|
||||||
|
|
||||||
|
_receiveTask?.Dispose();
|
||||||
|
_receiveTask = null;
|
||||||
|
|
||||||
|
_cts?.Dispose();
|
||||||
|
_cts = null;
|
||||||
|
|
||||||
|
_udpClient?.Dispose();
|
||||||
|
_udpClient = null;
|
||||||
|
|
||||||
|
lock (_commandLock)
|
||||||
|
{
|
||||||
|
_currentCommand = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
lock (_responseLock)
|
||||||
|
{
|
||||||
|
_latestResponse = null;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 释放客户端资源。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
if (_disposed)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_disposed = true;
|
||||||
|
_cts?.Cancel();
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_sendTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
|
}
|
||||||
|
catch (AggregateException)
|
||||||
|
{
|
||||||
|
// 忽略取消异常。
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_receiveTask?.Wait(TimeSpan.FromSeconds(1));
|
||||||
|
}
|
||||||
|
catch (AggregateException)
|
||||||
|
{
|
||||||
|
// 忽略取消异常。
|
||||||
|
}
|
||||||
|
|
||||||
|
_sendTask?.Dispose();
|
||||||
|
_receiveTask?.Dispose();
|
||||||
|
_cts?.Dispose();
|
||||||
|
_udpClient?.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 后台发送循环:约 8ms 周期发送当前命令。
|
||||||
|
/// </summary>
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
// 正常取消,退出循环。
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 后台接收循环:持续接收 132B 响应并解析。
|
||||||
|
/// </summary>
|
||||||
|
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
if (_udpClient is null)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
while (!cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
var result = await _udpClient.ReceiveAsync(cancellationToken).ConfigureAwait(false);
|
||||||
|
if (result.Buffer.Length == FanucJ519Protocol.ResponsePacketLength)
|
||||||
|
{
|
||||||
|
var response = FanucJ519Protocol.ParseResponse(result.Buffer);
|
||||||
|
lock (_responseLock)
|
||||||
|
{
|
||||||
|
_latestResponse = response;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// 正常取消,退出循环。
|
||||||
|
}
|
||||||
|
catch (ObjectDisposedException)
|
||||||
|
{
|
||||||
|
// UDP 客户端已释放,退出循环。
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
386
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
386
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
@@ -0,0 +1,386 @@
|
|||||||
|
using System.Buffers.Binary;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucJ519Command
|
||||||
|
{
|
||||||
|
private readonly double[] _targetJoints;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化 J519 命令数据。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="sequence">命令序号。</param>
|
||||||
|
/// <param name="targetJoints">目标关节或扩展轴数据,最多 9 个槽位。</param>
|
||||||
|
/// <param name="lastData">是否为最后一帧数据。</param>
|
||||||
|
/// <param name="readIoType">读取 IO 类型。</param>
|
||||||
|
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||||
|
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||||
|
/// <param name="dataStyle">目标数据类型。</param>
|
||||||
|
/// <param name="writeIoType">写入 IO 类型。</param>
|
||||||
|
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
|
||||||
|
/// <param name="writeIoMask">写入 IO 掩码。</param>
|
||||||
|
/// <param name="writeIoValue">写入 IO 数值。</param>
|
||||||
|
public FanucJ519Command(
|
||||||
|
uint sequence,
|
||||||
|
IReadOnlyList<double> targetJoints,
|
||||||
|
byte lastData = 0,
|
||||||
|
byte readIoType = 2,
|
||||||
|
ushort readIoIndex = 1,
|
||||||
|
ushort readIoMask = 255,
|
||||||
|
byte dataStyle = 1,
|
||||||
|
byte writeIoType = 2,
|
||||||
|
ushort writeIoIndex = 1,
|
||||||
|
ushort writeIoMask = 0,
|
||||||
|
ushort writeIoValue = 0)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||||
|
if (targetJoints.Count is <= 0 or > 9)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(nameof(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。");
|
||||||
|
}
|
||||||
|
|
||||||
|
Sequence = sequence;
|
||||||
|
LastData = lastData;
|
||||||
|
ReadIoType = readIoType;
|
||||||
|
ReadIoIndex = readIoIndex;
|
||||||
|
ReadIoMask = readIoMask;
|
||||||
|
DataStyle = dataStyle;
|
||||||
|
WriteIoType = writeIoType;
|
||||||
|
WriteIoIndex = writeIoIndex;
|
||||||
|
WriteIoMask = writeIoMask;
|
||||||
|
WriteIoValue = writeIoValue;
|
||||||
|
_targetJoints = targetJoints.ToArray();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取命令序号。
|
||||||
|
/// </summary>
|
||||||
|
public uint Sequence { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取是否为最后一帧数据。
|
||||||
|
/// </summary>
|
||||||
|
public byte LastData { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 类型。
|
||||||
|
/// </summary>
|
||||||
|
public byte ReadIoType { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 起始索引。
|
||||||
|
/// </summary>
|
||||||
|
public ushort ReadIoIndex { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 掩码。
|
||||||
|
/// </summary>
|
||||||
|
public ushort ReadIoMask { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取目标数据类型。
|
||||||
|
/// </summary>
|
||||||
|
public byte DataStyle { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取写入 IO 类型。
|
||||||
|
/// </summary>
|
||||||
|
public byte WriteIoType { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取写入 IO 起始索引。
|
||||||
|
/// </summary>
|
||||||
|
public ushort WriteIoIndex { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取写入 IO 掩码。
|
||||||
|
/// </summary>
|
||||||
|
public ushort WriteIoMask { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取写入 IO 数值。
|
||||||
|
/// </summary>
|
||||||
|
public ushort WriteIoValue { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取目标关节或扩展轴数据。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> TargetJoints => _targetJoints;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucJ519Response
|
||||||
|
{
|
||||||
|
private readonly double[] _pose;
|
||||||
|
private readonly double[] _externalAxes;
|
||||||
|
private readonly double[] _jointDegrees;
|
||||||
|
private readonly double[] _motorCurrents;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化 J519 响应数据。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageType">响应类型。</param>
|
||||||
|
/// <param name="version">协议版本。</param>
|
||||||
|
/// <param name="sequence">响应序号。</param>
|
||||||
|
/// <param name="status">状态位集合。</param>
|
||||||
|
/// <param name="readIoType">读取 IO 类型。</param>
|
||||||
|
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||||
|
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||||
|
/// <param name="readIoValue">读取 IO 数值。</param>
|
||||||
|
/// <param name="timestamp">控制器时间戳。</param>
|
||||||
|
/// <param name="pose">TCP 笛卡尔位姿。</param>
|
||||||
|
/// <param name="externalAxes">扩展轴反馈。</param>
|
||||||
|
/// <param name="jointDegrees">关节角度反馈。</param>
|
||||||
|
/// <param name="motorCurrents">电机电流反馈。</param>
|
||||||
|
public FanucJ519Response(
|
||||||
|
uint messageType,
|
||||||
|
uint version,
|
||||||
|
uint sequence,
|
||||||
|
byte status,
|
||||||
|
byte readIoType,
|
||||||
|
ushort readIoIndex,
|
||||||
|
ushort readIoMask,
|
||||||
|
ushort readIoValue,
|
||||||
|
uint timestamp,
|
||||||
|
IEnumerable<double> pose,
|
||||||
|
IEnumerable<double> externalAxes,
|
||||||
|
IEnumerable<double> jointDegrees,
|
||||||
|
IEnumerable<double> motorCurrents)
|
||||||
|
{
|
||||||
|
MessageType = messageType;
|
||||||
|
Version = version;
|
||||||
|
Sequence = sequence;
|
||||||
|
Status = status;
|
||||||
|
ReadIoType = readIoType;
|
||||||
|
ReadIoIndex = readIoIndex;
|
||||||
|
ReadIoMask = readIoMask;
|
||||||
|
ReadIoValue = readIoValue;
|
||||||
|
Timestamp = timestamp;
|
||||||
|
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||||
|
_externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
|
||||||
|
_jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
|
||||||
|
_motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取响应类型。
|
||||||
|
/// </summary>
|
||||||
|
public uint MessageType { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取协议版本。
|
||||||
|
/// </summary>
|
||||||
|
public uint Version { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取响应序号。
|
||||||
|
/// </summary>
|
||||||
|
public uint Sequence { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取状态位集合。
|
||||||
|
/// </summary>
|
||||||
|
public byte Status { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 类型。
|
||||||
|
/// </summary>
|
||||||
|
public byte ReadIoType { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 起始索引。
|
||||||
|
/// </summary>
|
||||||
|
public ushort ReadIoIndex { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 掩码。
|
||||||
|
/// </summary>
|
||||||
|
public ushort ReadIoMask { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取读取 IO 数值。
|
||||||
|
/// </summary>
|
||||||
|
public ushort ReadIoValue { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器时间戳。
|
||||||
|
/// </summary>
|
||||||
|
public uint Timestamp { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取 TCP 笛卡尔位姿。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> Pose => _pose;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取扩展轴反馈。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取关节角度反馈。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取电机电流反馈。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> MotorCurrents => _motorCurrents;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器是否接受命令。
|
||||||
|
/// </summary>
|
||||||
|
public bool AcceptsCommand => (Status & 0b0001) != 0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器是否已收到命令。
|
||||||
|
/// </summary>
|
||||||
|
public bool ReceivedCommand => (Status & 0b0010) != 0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器系统是否就绪。
|
||||||
|
/// </summary>
|
||||||
|
public bool SystemReady => (Status & 0b0100) != 0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取机器人是否处于运动中。
|
||||||
|
/// </summary>
|
||||||
|
public bool RobotInMotion => (Status & 0b1000) != 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
|
||||||
|
/// </summary>
|
||||||
|
public static class FanucJ519Protocol
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// J519 初始化和结束控制包长度。
|
||||||
|
/// </summary>
|
||||||
|
public const int ControlPacketLength = 8;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// J519 命令包长度。
|
||||||
|
/// </summary>
|
||||||
|
public const int CommandPacketLength = 64;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// J519 响应包长度。
|
||||||
|
/// </summary>
|
||||||
|
public const int ResponsePacketLength = 132;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 封装 J519 初始化包。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>初始化包。</returns>
|
||||||
|
public static byte[] PackInitPacket()
|
||||||
|
{
|
||||||
|
return PackControlPacket(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 封装 J519 结束包。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>结束包。</returns>
|
||||||
|
public static byte[] PackEndPacket()
|
||||||
|
{
|
||||||
|
return PackControlPacket(2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 封装 J519 64 字节命令包。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="command">命令数据。</param>
|
||||||
|
/// <returns>命令包。</returns>
|
||||||
|
public static byte[] PackCommandPacket(FanucJ519Command command)
|
||||||
|
{
|
||||||
|
ArgumentNullException.ThrowIfNull(command);
|
||||||
|
|
||||||
|
var packet = new byte[CommandPacketLength];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, sizeof(uint)), 1);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, sizeof(uint)), 1);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, sizeof(uint)), command.Sequence);
|
||||||
|
packet[0x0c] = command.LastData;
|
||||||
|
packet[0x0d] = command.ReadIoType;
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, sizeof(ushort)), command.ReadIoMask);
|
||||||
|
packet[0x12] = command.DataStyle;
|
||||||
|
packet[0x13] = command.WriteIoType;
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x14, sizeof(ushort)), command.WriteIoIndex);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x16, sizeof(ushort)), command.WriteIoMask);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x18, sizeof(ushort)), command.WriteIoValue);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x1a, sizeof(ushort)), 0);
|
||||||
|
|
||||||
|
// J519 命令包固定保留 9 个 f32 目标槽位,少于 9 个时剩余槽位补零。
|
||||||
|
for (var index = 0; index < 9; index++)
|
||||||
|
{
|
||||||
|
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
||||||
|
}
|
||||||
|
|
||||||
|
return packet;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析 J519 132 字节响应包。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="packet">响应包。</param>
|
||||||
|
/// <returns>响应解析结果。</returns>
|
||||||
|
public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
|
||||||
|
{
|
||||||
|
if (packet.Length != ResponsePacketLength)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC J519 响应包长度不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
return new FanucJ519Response(
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
|
||||||
|
packet[0x0c],
|
||||||
|
packet[0x0d],
|
||||||
|
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
|
||||||
|
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
|
||||||
|
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
|
||||||
|
ReadFloatArray(packet, 0x18, 6),
|
||||||
|
ReadFloatArray(packet, 0x30, 3),
|
||||||
|
ReadFloatArray(packet, 0x3c, 9),
|
||||||
|
ReadFloatArray(packet, 0x60, 9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 封装 J519 控制包。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="packetType">控制包类型。</param>
|
||||||
|
/// <returns>控制包。</returns>
|
||||||
|
private static byte[] PackControlPacket(uint packetType)
|
||||||
|
{
|
||||||
|
var packet = new byte[ControlPacketLength];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
|
||||||
|
return packet;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从响应包中读取固定长度 f32 数组。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="packet">响应包。</param>
|
||||||
|
/// <param name="offset">数组起始偏移。</param>
|
||||||
|
/// <param name="count">数组元素数量。</param>
|
||||||
|
/// <returns>转换成 double 的数值数组。</returns>
|
||||||
|
private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
|
||||||
|
{
|
||||||
|
var values = new double[count];
|
||||||
|
for (var index = 0; index < count; index++)
|
||||||
|
{
|
||||||
|
values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
|
||||||
|
}
|
||||||
|
|
||||||
|
return values;
|
||||||
|
}
|
||||||
|
}
|
||||||
188
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
188
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
@@ -0,0 +1,188 @@
|
|||||||
|
using System.Net.Sockets;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// FANUC TCP 10010 状态通道客户端,持续接收状态帧并缓存最新快照。
|
||||||
|
/// </summary>
|
||||||
|
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;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取当前是否已建立连接。
|
||||||
|
/// </summary>
|
||||||
|
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="ip">控制柜 IP 地址。</param>
|
||||||
|
/// <param name="port">状态通道端口,默认 10010。</param>
|
||||||
|
/// <param name="cancellationToken">取消令牌。</param>
|
||||||
|
public async Task ConnectAsync(string ip, int port = 10010, CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
if (string.IsNullOrWhiteSpace(ip))
|
||||||
|
{
|
||||||
|
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 断开状态通道并停止后台接收循环。
|
||||||
|
/// </summary>
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取最近一次解析的状态帧;若尚未收到任何帧则返回 null。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>最新状态帧或 null。</returns>
|
||||||
|
public FanucStateFrame? GetLatestFrame()
|
||||||
|
{
|
||||||
|
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||||
|
|
||||||
|
lock (_stateLock)
|
||||||
|
{
|
||||||
|
return _latestFrame;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 释放客户端资源。
|
||||||
|
/// </summary>
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 后台循环:持续从流中读取固定长度状态帧并更新缓存。
|
||||||
|
/// </summary>
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
// 解析到异常帧,退出循环由上层重连。
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从流中精确读取固定长度字节。
|
||||||
|
/// </summary>
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
127
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
127
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
@@ -0,0 +1,127 @@
|
|||||||
|
using System.Buffers.Binary;
|
||||||
|
|
||||||
|
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucStateFrame
|
||||||
|
{
|
||||||
|
private readonly double[] _pose;
|
||||||
|
private readonly double[] _jointOrExtensionValues;
|
||||||
|
private readonly uint[] _tailWords;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化状态帧解析结果。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="messageId">状态帧消息号或序号。</param>
|
||||||
|
/// <param name="pose">控制器回传的笛卡尔位姿。</param>
|
||||||
|
/// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
|
||||||
|
/// <param name="tailWords">状态帧尾部状态槽位。</param>
|
||||||
|
public FanucStateFrame(
|
||||||
|
uint messageId,
|
||||||
|
IEnumerable<double> pose,
|
||||||
|
IEnumerable<double> jointOrExtensionValues,
|
||||||
|
IEnumerable<uint> tailWords)
|
||||||
|
{
|
||||||
|
MessageId = messageId;
|
||||||
|
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||||
|
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
|
||||||
|
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取状态帧消息号或序号。
|
||||||
|
/// </summary>
|
||||||
|
public uint MessageId { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器回传的笛卡尔位姿。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> Pose => _pose;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取控制器回传的关节或扩展轴状态。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取状态帧尾部状态槽位。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<uint> TailWords => _tailWords;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
|
||||||
|
/// </summary>
|
||||||
|
public static class FanucStateProtocol
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// FANUC 状态通道抓包确认的完整帧长度。
|
||||||
|
/// </summary>
|
||||||
|
public const int StateFrameLength = 90;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析 TCP 10010 状态通道中的单个完整状态帧。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整状态帧。</param>
|
||||||
|
/// <returns>状态帧解析结果。</returns>
|
||||||
|
public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
ValidateFrame(frame);
|
||||||
|
|
||||||
|
var pose = new double[6];
|
||||||
|
var jointOrExtensionValues = new double[9];
|
||||||
|
var tailWords = new uint[4];
|
||||||
|
|
||||||
|
// 状态帧采用固定布局,偏移来自抓包与 StateServer 逆向结论。
|
||||||
|
for (var index = 0; index < pose.Length; index++)
|
||||||
|
{
|
||||||
|
pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
|
||||||
|
}
|
||||||
|
|
||||||
|
for (var index = 0; index < jointOrExtensionValues.Length; index++)
|
||||||
|
{
|
||||||
|
jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
|
||||||
|
}
|
||||||
|
|
||||||
|
for (var index = 0; index < tailWords.Length; index++)
|
||||||
|
{
|
||||||
|
tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
|
||||||
|
}
|
||||||
|
|
||||||
|
return new FanucStateFrame(
|
||||||
|
BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
|
||||||
|
pose,
|
||||||
|
jointOrExtensionValues,
|
||||||
|
tailWords);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 校验状态帧的长度、magic 和长度字段。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="frame">完整状态帧。</param>
|
||||||
|
private static void ValidateFrame(ReadOnlySpan<byte> frame)
|
||||||
|
{
|
||||||
|
if (frame.Length != StateFrameLength)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 状态帧头 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 状态帧尾 magic 不正确。");
|
||||||
|
}
|
||||||
|
|
||||||
|
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||||
|
if (declaredLength != frame.Length)
|
||||||
|
{
|
||||||
|
throw new InvalidDataException("FANUC 状态帧长度字段与实际长度不一致。");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -51,6 +51,26 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `GetServerVersion` 版本查询语义。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>服务端版本号。</returns>
|
||||||
|
[HttpGet("/get_server_version/")]
|
||||||
|
public IActionResult GetServerVersion()
|
||||||
|
{
|
||||||
|
return Ok(new { server_version = _compatService.GetServerVersion() });
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `GetClientVersion` 版本查询语义。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>客户端版本号。</returns>
|
||||||
|
[HttpGet("/get_client_version/")]
|
||||||
|
public IActionResult GetClientVersion()
|
||||||
|
{
|
||||||
|
return Ok(new { client_version = _compatService.GetClientVersion() });
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/setup_robot/` 路由。
|
/// 兼容旧 `/setup_robot/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -70,6 +90,49 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `SetUpRobotFromEnv(env_file)` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="env_file">环境文件路径。</param>
|
||||||
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
|
[HttpPost("/setup_robot_from_env/")]
|
||||||
|
public IActionResult SetupRobotFromEnv([FromQuery] string env_file)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_compatService.SetUpRobotFromEnv(env_file);
|
||||||
|
return Ok(new { status = "robot setup" });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("SetUpRobotFromEnv failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `SetShowTCP(is_show, axis_length, axis_size)` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="is_show">是否显示 TCP。</param>
|
||||||
|
/// <param name="axis_length">坐标轴长度。</param>
|
||||||
|
/// <param name="axis_size">坐标轴线宽。</param>
|
||||||
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
|
[HttpPost("/set_show_tcp/")]
|
||||||
|
public IActionResult SetShowTcp(
|
||||||
|
[FromQuery] bool is_show = true,
|
||||||
|
[FromQuery] double axis_length = 0.1,
|
||||||
|
[FromQuery] int axis_size = 2)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_compatService.SetShowTcp(is_show, axis_length, axis_size);
|
||||||
|
return Ok(new { status = "show TCP set" });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("SetShowTCP failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/is_setup/` 路由。
|
/// 兼容旧 `/is_setup/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -81,15 +144,16 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/enable_robot/` 路由;保持原 Python 服务固定传 `8` 的行为。
|
/// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
/// <param name="buffer_size">控制器执行缓冲区大小。</param>
|
||||||
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
|
||||||
[HttpGet("/enable_robot/")]
|
[HttpGet("/enable_robot/")]
|
||||||
public IActionResult EnableRobot()
|
public IActionResult EnableRobot([FromQuery] int buffer_size = 2)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
_compatService.EnableRobot(8);
|
_compatService.EnableRobot(buffer_size);
|
||||||
return Ok(new { enable_robot = true });
|
return Ok(new { enable_robot = true });
|
||||||
}
|
}
|
||||||
catch
|
catch
|
||||||
@@ -116,6 +180,24 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供与旧客户端 <c>StopMove</c> 语义对应的 HTTP 端点。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
|
[HttpGet("/stop_move/")]
|
||||||
|
public IActionResult StopMove()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_compatService.StopMove();
|
||||||
|
return Ok(new { status = "move stopped" });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("StopMove failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/set_active_controller/` 路由。
|
/// 兼容旧 `/set_active_controller/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -154,6 +236,24 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供与旧客户端 <c>Disconnect</c> 语义对应的 HTTP 端点。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
|
[HttpPost("/disconnect_robot/")]
|
||||||
|
public IActionResult DisconnectRobot()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_compatService.Disconnect();
|
||||||
|
return Ok(new { status = "robot disconnected" });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("Disconnect failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/robot_info/` 路由。
|
/// 兼容旧 `/robot_info/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -234,6 +334,25 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `/get_io/` 路由。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="port">IO 端口号。</param>
|
||||||
|
/// <param name="io_type">IO 类型字符串。</param>
|
||||||
|
/// <returns>当前 IO 值。</returns>
|
||||||
|
[HttpGet("/get_io/")]
|
||||||
|
public IActionResult GetIo([FromQuery] int port, [FromQuery] string io_type)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
return Ok(new { value = _compatService.GetIo(port, io_type) });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("GetDigitalOutput failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/get_joint_position/` 路由。
|
/// 兼容旧 `/get_joint_position/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -270,6 +389,28 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="request">IK 请求体。</param>
|
||||||
|
/// <returns>IK 结果。</returns>
|
||||||
|
[HttpPost("/get_nearest_ik/")]
|
||||||
|
public IActionResult GetNearestIk([FromBody] LegacyNearestIkRequest request)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
return Ok(new { success = true, ik = _compatService.GetNearestIk(request.pose, request.seed) });
|
||||||
|
}
|
||||||
|
catch (NotSupportedException exception)
|
||||||
|
{
|
||||||
|
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("GetNearestIK failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/list_flyShotTraj/` 路由。
|
/// 兼容旧 `/list_flyShotTraj/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -292,11 +433,17 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
/// <param name="waypoints">轨迹请求体。</param>
|
/// <param name="waypoints">轨迹请求体。</param>
|
||||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
[HttpPost("/execute_trajectory/")]
|
[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
|
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" });
|
return Ok(new { status = "trajectory executed" });
|
||||||
}
|
}
|
||||||
catch
|
catch
|
||||||
@@ -349,14 +496,20 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/execute_flyshot/` 路由。
|
/// 兼容旧 `/execute_flyshot/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
/// <param name="data">包含轨迹名称的请求体。</param>
|
/// <param name="data">包含轨迹名称和执行参数的请求体。</param>
|
||||||
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
[HttpPost("/execute_flyshot/")]
|
[HttpPost("/execute_flyshot/")]
|
||||||
public IActionResult ExecuteFlyshot([FromBody] LegacyNameRequest data)
|
public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data)
|
||||||
{
|
{
|
||||||
try
|
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 });
|
return Ok(new { status = "FlyShot executed", success = true });
|
||||||
}
|
}
|
||||||
catch (Exception exception)
|
catch (Exception exception)
|
||||||
@@ -365,6 +518,57 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `SaveTrajInfo(name, method)` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="request">轨迹保存请求体。</param>
|
||||||
|
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
|
||||||
|
[HttpPost("/save_traj_info/")]
|
||||||
|
public IActionResult SaveTrajectoryInfo([FromBody] LegacyTrajectoryInfoRequest request)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
_compatService.SaveTrajectoryInfo(request.name, request.method);
|
||||||
|
return Ok(new { status = "trajectory info saved", success = true });
|
||||||
|
}
|
||||||
|
catch (NotSupportedException exception)
|
||||||
|
{
|
||||||
|
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("SaveTrajInfo failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 兼容旧 `IsFlyShotTrajValid(time, name, method, save_traj)` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="request">轨迹有效性检查请求体。</param>
|
||||||
|
/// <returns>有效性和轨迹时长。</returns>
|
||||||
|
[HttpPost("/is_flyShotTrajValid/")]
|
||||||
|
public IActionResult IsFlyshotTrajectoryValid([FromBody] LegacyFlyshotValidationRequest request)
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var isValid = _compatService.IsFlyshotTrajectoryValid(
|
||||||
|
out var duration,
|
||||||
|
request.name,
|
||||||
|
request.method,
|
||||||
|
request.save_traj);
|
||||||
|
|
||||||
|
return Ok(new { success = isValid, valid = isValid, time = duration.TotalSeconds });
|
||||||
|
}
|
||||||
|
catch (NotSupportedException exception)
|
||||||
|
{
|
||||||
|
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
|
||||||
|
}
|
||||||
|
catch
|
||||||
|
{
|
||||||
|
return LegacyBadRequest("IsFlyShotTrajValid failed");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 兼容旧 `/set_speedRatio/` 路由。
|
/// 兼容旧 `/set_speedRatio/` 路由。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -420,7 +624,7 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
return LegacyBadRequest("Robot not setup");
|
return LegacyBadRequest("Robot not setup");
|
||||||
}
|
}
|
||||||
|
|
||||||
_compatService.SetActiveController(sim: false);
|
_compatService.SetActiveController(data.sim);
|
||||||
_compatService.Connect(data.robot_ip);
|
_compatService.Connect(data.robot_ip);
|
||||||
_compatService.EnableRobot(2);
|
_compatService.EnableRobot(2);
|
||||||
return Ok(new { message = "init_Success", returnCode = 0 });
|
return Ok(new { message = "init_Success", returnCode = 0 });
|
||||||
@@ -448,6 +652,47 @@ public sealed class LegacyHttpApiController : ControllerBase
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 解析旧 `/execute_trajectory/` 的完整参数形状。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="payload">原始 JSON 请求体。</param>
|
||||||
|
/// <param name="queryMethod">查询字符串中的 method 覆盖值。</param>
|
||||||
|
/// <param name="querySaveTrajectory">查询字符串中的 save_traj 覆盖值。</param>
|
||||||
|
/// <returns>统一后的路点和执行参数。</returns>
|
||||||
|
private static (
|
||||||
|
IReadOnlyList<IReadOnlyList<double>> Waypoints,
|
||||||
|
string Method,
|
||||||
|
bool SaveTrajectory) ParseExecuteTrajectoryRequest(
|
||||||
|
JsonElement payload,
|
||||||
|
string? queryMethod,
|
||||||
|
bool? querySaveTrajectory)
|
||||||
|
{
|
||||||
|
string method = queryMethod ?? "icsp";
|
||||||
|
bool saveTrajectory = querySaveTrajectory ?? false;
|
||||||
|
|
||||||
|
if (payload.ValueKind == JsonValueKind.Object)
|
||||||
|
{
|
||||||
|
if (payload.TryGetProperty("method", out var methodElement) && methodElement.ValueKind == JsonValueKind.String)
|
||||||
|
{
|
||||||
|
method = methodElement.GetString() ?? method;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (payload.TryGetProperty("save_traj", out var saveTrajectoryElement))
|
||||||
|
{
|
||||||
|
saveTrajectory = saveTrajectoryElement.GetBoolean();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!payload.TryGetProperty("waypoints", out var waypointElement))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException("ExecuteTrajectory request body must include waypoints.");
|
||||||
|
}
|
||||||
|
|
||||||
|
return (ParseLegacyTrajectoryWaypoints(waypointElement), method, saveTrajectory);
|
||||||
|
}
|
||||||
|
|
||||||
|
return (ParseLegacyTrajectoryWaypoints(payload), method, saveTrajectory);
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 解析旧 `/execute_trajectory/` 可能出现的两种历史请求体形状。
|
/// 解析旧 `/execute_trajectory/` 可能出现的两种历史请求体形状。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -576,6 +821,90 @@ public sealed class LegacyNameRequest
|
|||||||
public string name { get; init; } = string.Empty;
|
public string name { get; init; } = string.Empty;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示旧 `/execute_flyshot/` 路由使用的完整执行请求体。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class LegacyExecuteFlyshotRequest
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹名称。
|
||||||
|
/// </summary>
|
||||||
|
public string name { get; init; } = string.Empty;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否先移动到轨迹起点。
|
||||||
|
/// </summary>
|
||||||
|
public bool move_to_start { get; init; } = true;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹生成方法。
|
||||||
|
/// </summary>
|
||||||
|
public string method { get; init; } = "icsp";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否保存轨迹信息。
|
||||||
|
/// </summary>
|
||||||
|
public bool save_traj { get; init; } = true;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否复用轨迹缓存。
|
||||||
|
/// </summary>
|
||||||
|
public bool use_cache { get; init; } = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示旧 `SaveTrajInfo` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class LegacyTrajectoryInfoRequest
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹名称。
|
||||||
|
/// </summary>
|
||||||
|
public string name { get; init; } = string.Empty;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹生成方法。
|
||||||
|
/// </summary>
|
||||||
|
public string method { get; init; } = "icsp";
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示旧 `IsFlyShotTrajValid` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class LegacyFlyshotValidationRequest
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹名称。
|
||||||
|
/// </summary>
|
||||||
|
public string name { get; init; } = string.Empty;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置轨迹生成方法。
|
||||||
|
/// </summary>
|
||||||
|
public string method { get; init; } = "icsp";
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否保存轨迹信息。
|
||||||
|
/// </summary>
|
||||||
|
public bool save_traj { get; init; } = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 表示旧 `GetNearestIK` 参数形状。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class LegacyNearestIkRequest
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置目标位姿 `[x,y,z,qx,qy,qz,qw]`。
|
||||||
|
/// </summary>
|
||||||
|
public List<double> pose { get; init; } = [];
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置 IK seed 关节数组。
|
||||||
|
/// </summary>
|
||||||
|
public List<double> seed { get; init; } = [];
|
||||||
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 表示旧 `/set_speedRatio/` 路由使用的速度倍率请求体。
|
/// 表示旧 `/set_speedRatio/` 路由使用的速度倍率请求体。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
@@ -611,4 +940,9 @@ public sealed class LegacyInitMpcRobotRequest
|
|||||||
/// 获取或设置机器人控制器 IP。
|
/// 获取或设置机器人控制器 IP。
|
||||||
/// </summary>
|
/// </summary>
|
||||||
public string robot_ip { get; init; } = string.Empty;
|
public string robot_ip { get; init; } = string.Empty;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取或设置是否使用仿真控制器;默认 false 连接真机。
|
||||||
|
/// </summary>
|
||||||
|
public bool sim { get; init; }
|
||||||
}
|
}
|
||||||
|
|||||||
385
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
385
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
@@ -0,0 +1,385 @@
|
|||||||
|
using Flyshot.ControllerClientCompat;
|
||||||
|
using Microsoft.AspNetCore.Mvc;
|
||||||
|
|
||||||
|
namespace Flyshot.Server.Host.Controllers;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 提供只读状态监控页面和控制器状态快照 API。
|
||||||
|
/// </summary>
|
||||||
|
[ApiController]
|
||||||
|
public sealed class StatusController : ControllerBase
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 浏览器端状态监控页面,页面逻辑只读取状态快照,不触发控制动作。
|
||||||
|
/// </summary>
|
||||||
|
private const string StatusPageHtml = """
|
||||||
|
<!doctype html>
|
||||||
|
<html lang="zh-CN">
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
|
<title>Flyshot Replacement 状态监控</title>
|
||||||
|
<style>
|
||||||
|
:root {
|
||||||
|
color-scheme: light;
|
||||||
|
--bg: #f5f7fb;
|
||||||
|
--surface: #ffffff;
|
||||||
|
--line: #d8dee9;
|
||||||
|
--text: #172033;
|
||||||
|
--muted: #5b667a;
|
||||||
|
--accent: #007c89;
|
||||||
|
--good: #12805c;
|
||||||
|
--warn: #b7791f;
|
||||||
|
--bad: #b42318;
|
||||||
|
}
|
||||||
|
|
||||||
|
* {
|
||||||
|
box-sizing: border-box;
|
||||||
|
}
|
||||||
|
|
||||||
|
body {
|
||||||
|
margin: 0;
|
||||||
|
min-height: 100vh;
|
||||||
|
background: var(--bg);
|
||||||
|
color: var(--text);
|
||||||
|
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
|
||||||
|
font-size: 15px;
|
||||||
|
letter-spacing: 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
header {
|
||||||
|
border-bottom: 1px solid var(--line);
|
||||||
|
background: var(--surface);
|
||||||
|
}
|
||||||
|
|
||||||
|
.topbar {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: space-between;
|
||||||
|
gap: 16px;
|
||||||
|
width: min(1180px, calc(100% - 32px));
|
||||||
|
margin: 0 auto;
|
||||||
|
padding: 18px 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
h1 {
|
||||||
|
margin: 0;
|
||||||
|
font-size: 22px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
button {
|
||||||
|
min-height: 36px;
|
||||||
|
padding: 0 14px;
|
||||||
|
border: 1px solid var(--accent);
|
||||||
|
border-radius: 6px;
|
||||||
|
background: var(--accent);
|
||||||
|
color: #ffffff;
|
||||||
|
font: inherit;
|
||||||
|
cursor: pointer;
|
||||||
|
}
|
||||||
|
|
||||||
|
button:disabled {
|
||||||
|
opacity: 0.6;
|
||||||
|
cursor: default;
|
||||||
|
}
|
||||||
|
|
||||||
|
main {
|
||||||
|
width: min(1180px, calc(100% - 32px));
|
||||||
|
margin: 22px auto;
|
||||||
|
}
|
||||||
|
|
||||||
|
.summary {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: repeat(4, minmax(0, 1fr));
|
||||||
|
gap: 12px;
|
||||||
|
margin-bottom: 16px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.metric,
|
||||||
|
section {
|
||||||
|
border: 1px solid var(--line);
|
||||||
|
border-radius: 8px;
|
||||||
|
background: var(--surface);
|
||||||
|
}
|
||||||
|
|
||||||
|
.metric {
|
||||||
|
min-height: 86px;
|
||||||
|
padding: 14px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.label {
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 13px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.value {
|
||||||
|
margin-top: 8px;
|
||||||
|
overflow-wrap: anywhere;
|
||||||
|
font-size: 24px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-row {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
gap: 8px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.dot {
|
||||||
|
width: 12px;
|
||||||
|
height: 12px;
|
||||||
|
flex: 0 0 12px;
|
||||||
|
border-radius: 999px;
|
||||||
|
background: var(--warn);
|
||||||
|
}
|
||||||
|
|
||||||
|
.dot.good {
|
||||||
|
background: var(--good);
|
||||||
|
}
|
||||||
|
|
||||||
|
.dot.bad {
|
||||||
|
background: var(--bad);
|
||||||
|
}
|
||||||
|
|
||||||
|
.grid {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||||
|
gap: 16px;
|
||||||
|
}
|
||||||
|
|
||||||
|
section h2 {
|
||||||
|
margin: 0;
|
||||||
|
padding: 14px 16px;
|
||||||
|
border-bottom: 1px solid var(--line);
|
||||||
|
font-size: 16px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
dl {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: 160px minmax(0, 1fr);
|
||||||
|
gap: 0;
|
||||||
|
margin: 0;
|
||||||
|
padding: 4px 16px 12px;
|
||||||
|
}
|
||||||
|
|
||||||
|
dt,
|
||||||
|
dd {
|
||||||
|
min-height: 36px;
|
||||||
|
margin: 0;
|
||||||
|
padding: 9px 0;
|
||||||
|
border-bottom: 1px solid #edf1f7;
|
||||||
|
}
|
||||||
|
|
||||||
|
dt {
|
||||||
|
color: var(--muted);
|
||||||
|
}
|
||||||
|
|
||||||
|
dd {
|
||||||
|
overflow-wrap: anywhere;
|
||||||
|
font-family: Consolas, "Cascadia Mono", monospace;
|
||||||
|
}
|
||||||
|
|
||||||
|
.empty {
|
||||||
|
color: var(--muted);
|
||||||
|
font-family: inherit;
|
||||||
|
}
|
||||||
|
|
||||||
|
@media (max-width: 820px) {
|
||||||
|
.topbar {
|
||||||
|
align-items: flex-start;
|
||||||
|
flex-direction: column;
|
||||||
|
}
|
||||||
|
|
||||||
|
.summary,
|
||||||
|
.grid {
|
||||||
|
grid-template-columns: 1fr;
|
||||||
|
}
|
||||||
|
|
||||||
|
dl {
|
||||||
|
grid-template-columns: 1fr;
|
||||||
|
}
|
||||||
|
|
||||||
|
dt {
|
||||||
|
border-bottom: 0;
|
||||||
|
padding-bottom: 2px;
|
||||||
|
}
|
||||||
|
|
||||||
|
dd {
|
||||||
|
padding-top: 2px;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<header>
|
||||||
|
<div class="topbar">
|
||||||
|
<h1>Flyshot Replacement 状态监控</h1>
|
||||||
|
<button id="refresh" type="button">刷新</button>
|
||||||
|
</div>
|
||||||
|
</header>
|
||||||
|
<main>
|
||||||
|
<div class="summary">
|
||||||
|
<div class="metric">
|
||||||
|
<div class="label">连接状态</div>
|
||||||
|
<div class="value status-row"><span id="state-dot" class="dot"></span><span id="connection-state">--</span></div>
|
||||||
|
</div>
|
||||||
|
<div class="metric">
|
||||||
|
<div class="label">机器人</div>
|
||||||
|
<div id="robot-name" class="value">--</div>
|
||||||
|
</div>
|
||||||
|
<div class="metric">
|
||||||
|
<div class="label">速度倍率</div>
|
||||||
|
<div id="speed-ratio" class="value">--</div>
|
||||||
|
</div>
|
||||||
|
<div class="metric">
|
||||||
|
<div class="label">运动中</div>
|
||||||
|
<div id="motion-state" class="value">--</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
<div class="grid">
|
||||||
|
<section>
|
||||||
|
<h2>控制器</h2>
|
||||||
|
<dl>
|
||||||
|
<dt>服务端版本</dt><dd id="server-version">--</dd>
|
||||||
|
<dt>客户端版本</dt><dd id="client-version">--</dd>
|
||||||
|
<dt>已初始化</dt><dd id="setup-state">--</dd>
|
||||||
|
<dt>已使能</dt><dd id="enabled-state">--</dd>
|
||||||
|
<dt>采样时间</dt><dd id="captured-at">--</dd>
|
||||||
|
</dl>
|
||||||
|
</section>
|
||||||
|
<section>
|
||||||
|
<h2>机器人</h2>
|
||||||
|
<dl>
|
||||||
|
<dt>自由度</dt><dd id="dof">--</dd>
|
||||||
|
<dt>关节位置</dt><dd id="joints">--</dd>
|
||||||
|
<dt>TCP 位姿</dt><dd id="pose">--</dd>
|
||||||
|
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
|
||||||
|
</dl>
|
||||||
|
</section>
|
||||||
|
</div>
|
||||||
|
</main>
|
||||||
|
<script>
|
||||||
|
const fields = {
|
||||||
|
connectionState: document.getElementById("connection-state"),
|
||||||
|
stateDot: document.getElementById("state-dot"),
|
||||||
|
robotName: document.getElementById("robot-name"),
|
||||||
|
speedRatio: document.getElementById("speed-ratio"),
|
||||||
|
motionState: document.getElementById("motion-state"),
|
||||||
|
serverVersion: document.getElementById("server-version"),
|
||||||
|
clientVersion: document.getElementById("client-version"),
|
||||||
|
setupState: document.getElementById("setup-state"),
|
||||||
|
enabledState: document.getElementById("enabled-state"),
|
||||||
|
capturedAt: document.getElementById("captured-at"),
|
||||||
|
dof: document.getElementById("dof"),
|
||||||
|
joints: document.getElementById("joints"),
|
||||||
|
pose: document.getElementById("pose"),
|
||||||
|
trajectories: document.getElementById("trajectories"),
|
||||||
|
refresh: document.getElementById("refresh")
|
||||||
|
};
|
||||||
|
|
||||||
|
function formatArray(values) {
|
||||||
|
if (!Array.isArray(values) || values.length === 0) {
|
||||||
|
return "--";
|
||||||
|
}
|
||||||
|
|
||||||
|
return values.map(value => Number(value).toFixed(4)).join(", ");
|
||||||
|
}
|
||||||
|
|
||||||
|
function setDot(connectionState) {
|
||||||
|
fields.stateDot.className = "dot";
|
||||||
|
if (connectionState === "Connected") {
|
||||||
|
fields.stateDot.classList.add("good");
|
||||||
|
} else if (connectionState === "NotConfigured") {
|
||||||
|
fields.stateDot.classList.add("bad");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
async function refreshStatus() {
|
||||||
|
fields.refresh.disabled = true;
|
||||||
|
try {
|
||||||
|
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
|
||||||
|
const payload = await response.json();
|
||||||
|
const snapshot = payload.snapshot;
|
||||||
|
|
||||||
|
fields.connectionState.textContent = snapshot.connectionState;
|
||||||
|
fields.robotName.textContent = payload.robotName || "--";
|
||||||
|
fields.speedRatio.textContent = Number(snapshot.speedRatio).toFixed(2);
|
||||||
|
fields.motionState.textContent = snapshot.isInMotion ? "是" : "否";
|
||||||
|
fields.serverVersion.textContent = payload.serverVersion;
|
||||||
|
fields.clientVersion.textContent = payload.clientVersion;
|
||||||
|
fields.setupState.textContent = payload.isSetup ? "是" : "否";
|
||||||
|
fields.enabledState.textContent = snapshot.isEnabled ? "是" : "否";
|
||||||
|
fields.capturedAt.textContent = new Date(snapshot.capturedAt).toLocaleString();
|
||||||
|
fields.dof.textContent = payload.degreesOfFreedom;
|
||||||
|
fields.joints.textContent = formatArray(snapshot.jointPositions);
|
||||||
|
fields.pose.textContent = formatArray(snapshot.cartesianPose);
|
||||||
|
fields.trajectories.textContent = payload.uploadedTrajectories.length > 0
|
||||||
|
? payload.uploadedTrajectories.join(", ")
|
||||||
|
: "--";
|
||||||
|
fields.trajectories.classList.toggle("empty", payload.uploadedTrajectories.length === 0);
|
||||||
|
setDot(snapshot.connectionState);
|
||||||
|
} finally {
|
||||||
|
fields.refresh.disabled = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fields.refresh.addEventListener("click", refreshStatus);
|
||||||
|
refreshStatus();
|
||||||
|
window.setInterval(refreshStatus, 2000);
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
|
""";
|
||||||
|
|
||||||
|
private readonly IControllerClientCompatService _compatService;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化状态监控控制器。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="compatService">ControllerClient 兼容层服务。</param>
|
||||||
|
public StatusController(IControllerClientCompatService compatService)
|
||||||
|
{
|
||||||
|
_compatService = compatService ?? throw new ArgumentNullException(nameof(compatService));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 返回浏览器可直接打开的状态监控页面。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>HTML 状态页面。</returns>
|
||||||
|
[HttpGet("/status")]
|
||||||
|
public ContentResult GetStatusPage()
|
||||||
|
{
|
||||||
|
return Content(StatusPageHtml, "text/html; charset=utf-8");
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 返回当前 ControllerClient 兼容层与控制器运行时状态快照。
|
||||||
|
/// </summary>
|
||||||
|
/// <returns>面向状态页和外部诊断的 JSON 快照。</returns>
|
||||||
|
[HttpGet("/api/status/snapshot")]
|
||||||
|
public IActionResult GetSnapshot()
|
||||||
|
{
|
||||||
|
var snapshot = _compatService.GetControllerSnapshot();
|
||||||
|
var isSetup = _compatService.IsSetUp;
|
||||||
|
|
||||||
|
// 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。
|
||||||
|
var robotName = isSetup ? _compatService.GetRobotName() : null;
|
||||||
|
var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0;
|
||||||
|
var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty<string>();
|
||||||
|
|
||||||
|
return Ok(new
|
||||||
|
{
|
||||||
|
Status = "ok",
|
||||||
|
Service = "flyshot-server-host",
|
||||||
|
ServerVersion = _compatService.GetServerVersion(),
|
||||||
|
ClientVersion = _compatService.GetClientVersion(),
|
||||||
|
IsSetup = isSetup,
|
||||||
|
RobotName = robotName,
|
||||||
|
DegreesOfFreedom = degreesOfFreedom,
|
||||||
|
UploadedTrajectories = uploadedTrajectories,
|
||||||
|
Snapshot = snapshot
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
179
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs
Normal file
179
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs
Normal file
@@ -0,0 +1,179 @@
|
|||||||
|
using System.Net;
|
||||||
|
using System.Net.Sockets;
|
||||||
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 FANUC TCP 10012 命令客户端的帧收发与响应解析。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucCommandClientTests : IDisposable
|
||||||
|
{
|
||||||
|
private readonly TcpListener _listener;
|
||||||
|
private readonly CancellationTokenSource _cts = new();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 在随机可用端口启动本地模拟控制器。
|
||||||
|
/// </summary>
|
||||||
|
public FanucCommandClientTests()
|
||||||
|
{
|
||||||
|
_listener = new TcpListener(IPAddress.Loopback, 0);
|
||||||
|
_listener.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取分配给本地模拟控制器的端口。
|
||||||
|
/// </summary>
|
||||||
|
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 清理模拟控制器和取消源。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
_cts.Cancel();
|
||||||
|
_listener.Stop();
|
||||||
|
_cts.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证命令客户端可以连接本地模拟控制器。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ConnectAsync_ConnectsToLocalListener()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
var acceptTask = _listener.AcceptTcpClientAsync();
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(client.IsConnected);
|
||||||
|
// 确保模拟侧也完成握手
|
||||||
|
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 StopProgram 命令帧与抓包样本一致,并能解析成功响应。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StopProgramAsync_SendsCorrectFrameAndParsesSuccess()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
var handlerTask = RunSingleResponseControllerAsync(
|
||||||
|
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
|
||||||
|
Convert.FromHexString("646f7a0000001200002103000000007a6f64"),
|
||||||
|
_cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var response = await client.StopProgramAsync("RVBUSTSM", _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(response.IsSuccess);
|
||||||
|
Assert.Equal(FanucCommandMessageIds.StopProgram, response.MessageId);
|
||||||
|
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 ResetRobot 空命令帧能正确发送并解析结果响应。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ResetRobotAsync_SendsEmptyCommandAndParsesResponse()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
var expectedFrame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||||
|
var responseFrame = Convert.FromHexString("646f7a0000001200002100000000007a6f64");
|
||||||
|
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var response = await client.ResetRobotAsync(_cts.Token);
|
||||||
|
|
||||||
|
Assert.True(response.IsSuccess);
|
||||||
|
Assert.Equal(FanucCommandMessageIds.ResetRobot, response.MessageId);
|
||||||
|
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 GetProgramStatus 命令帧能正确发送并解析程序状态响应。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task GetProgramStatusAsync_SendsFrameAndParsesStatusResponse()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
|
||||||
|
var responseFrame = Convert.FromHexString("646f7a000000160000200300000000000000017a6f64");
|
||||||
|
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var response = await client.GetProgramStatusAsync("RVBUSTSM", _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(response.IsSuccess);
|
||||||
|
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, response.MessageId);
|
||||||
|
Assert.Equal(1u, response.ProgramStatus);
|
||||||
|
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 StartProgram 命令帧能正确发送并解析成功响应。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartProgramAsync_SendsCorrectFrameAndParsesSuccess()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StartProgram, "RVBUSTSM");
|
||||||
|
var responseFrame = Convert.FromHexString("646f7a0000001200002102000000007a6f64");
|
||||||
|
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
var response = await client.StartProgramAsync("RVBUSTSM", _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(response.IsSuccess);
|
||||||
|
Assert.Equal(FanucCommandMessageIds.StartProgram, response.MessageId);
|
||||||
|
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证在连接前调用命令会抛出 InvalidOperationException。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task SendProgramCommandAsync_BeforeConnect_Throws()
|
||||||
|
{
|
||||||
|
using var client = new FanucCommandClient();
|
||||||
|
await Assert.ThrowsAsync<InvalidOperationException>(
|
||||||
|
() => client.StopProgramAsync("RVBUSTSM", _cts.Token));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 启动模拟控制器,接收一条请求帧并比对期望内容,然后返回预设响应。
|
||||||
|
/// </summary>
|
||||||
|
private async Task RunSingleResponseControllerAsync(
|
||||||
|
byte[] expectedFrame,
|
||||||
|
byte[] responseFrame,
|
||||||
|
CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||||
|
await using var stream = controller.GetStream();
|
||||||
|
|
||||||
|
var buffer = new byte[expectedFrame.Length];
|
||||||
|
await ReadExactAsync(stream, buffer, cancellationToken);
|
||||||
|
Assert.Equal(expectedFrame, buffer);
|
||||||
|
|
||||||
|
await stream.WriteAsync(responseFrame, cancellationToken);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 从流中精确读取指定长度的字节。
|
||||||
|
/// </summary>
|
||||||
|
private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
var totalRead = 0;
|
||||||
|
while (totalRead < buffer.Length)
|
||||||
|
{
|
||||||
|
var read = await stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken);
|
||||||
|
if (read == 0)
|
||||||
|
{
|
||||||
|
throw new IOException("模拟控制器读取到 EOF。");
|
||||||
|
}
|
||||||
|
|
||||||
|
totalRead += read;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
180
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
180
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
Normal file
@@ -0,0 +1,180 @@
|
|||||||
|
using System.Buffers.Binary;
|
||||||
|
using System.Net;
|
||||||
|
using System.Net.Sockets;
|
||||||
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucJ519ClientTests : IDisposable
|
||||||
|
{
|
||||||
|
private readonly UdpClient _server;
|
||||||
|
private readonly CancellationTokenSource _cts = new();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 在随机可用端口启动本地 UDP 模拟控制器。
|
||||||
|
/// </summary>
|
||||||
|
public FanucJ519ClientTests()
|
||||||
|
{
|
||||||
|
_server = new UdpClient(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取分配给本地模拟控制器的端口。
|
||||||
|
/// </summary>
|
||||||
|
private int Port => ((IPEndPoint)_server.Client.LocalEndPoint!).Port;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 清理模拟控制器和取消源。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
_cts.Cancel();
|
||||||
|
_server.Dispose();
|
||||||
|
_cts.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证连接时会发送初始化包。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ConnectAsync_SendsInitPacket()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
var receiveTask = _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(client.IsConnected);
|
||||||
|
var result = await receiveTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
Assert.Equal(FanucJ519Protocol.ControlPacketLength, result.Buffer.Length);
|
||||||
|
Assert.Equal(Convert.FromHexString("0000000000000001"), result.Buffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证启动运动后能按周期发送命令包。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StartMotion_SendsPeriodicCommands()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
// 接收并丢弃初始化包。
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||||
|
client.UpdateCommand(command);
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
// 接收至少一个命令包。
|
||||||
|
var commandResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
|
||||||
|
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证停止运动时会发送结束包。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task StopMotionAsync_SendsEndPacket()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
// 接收并丢弃初始化包。
|
||||||
|
await _server.ReceiveAsync(_cts.Token);
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
|
||||||
|
// 服务器应该收到结束包。
|
||||||
|
var endResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(FanucJ519Protocol.ControlPacketLength, endResult.Buffer.Length);
|
||||||
|
Assert.Equal(Convert.FromHexString("0000000200000001"), endResult.Buffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证响应解析和最新响应缓存。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task GetLatestResponse_ParsesIncomingResponse()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
// 接收初始化包,获取客户端端点。
|
||||||
|
var initResult = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
var clientEndpoint = initResult.RemoteEndPoint;
|
||||||
|
|
||||||
|
// 构造 132B 响应包并发送回客户端。
|
||||||
|
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), 5);
|
||||||
|
responsePacket[0x0c] = 15; // 所有状态位为真。
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x14, 4), 999u);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x18, 4), 10.0f);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x3c, 4), 0.5f);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x60, 4), 1.0f);
|
||||||
|
|
||||||
|
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
|
||||||
|
|
||||||
|
// 给接收循环留出时间。
|
||||||
|
await Task.Delay(200, _cts.Token);
|
||||||
|
|
||||||
|
var latest = client.GetLatestResponse();
|
||||||
|
Assert.NotNull(latest);
|
||||||
|
Assert.Equal(5u, latest.Sequence);
|
||||||
|
Assert.True(latest.AcceptsCommand);
|
||||||
|
Assert.True(latest.ReceivedCommand);
|
||||||
|
Assert.True(latest.SystemReady);
|
||||||
|
Assert.True(latest.RobotInMotion);
|
||||||
|
Assert.Equal(999u, latest.Timestamp);
|
||||||
|
Assert.Equal(10.0, latest.Pose[0], precision: 6);
|
||||||
|
Assert.Equal(0.5, latest.JointDegrees[0], precision: 6);
|
||||||
|
Assert.Equal(1.0, latest.MotorCurrents[0], precision: 6);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task UpdateCommand_ReplacesCurrentCommand()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
await _server.ReceiveAsync(_cts.Token); // init
|
||||||
|
|
||||||
|
var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||||
|
var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||||
|
|
||||||
|
client.UpdateCommand(command1);
|
||||||
|
client.StartMotion();
|
||||||
|
|
||||||
|
var result1 = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
|
|
||||||
|
client.UpdateCommand(command2);
|
||||||
|
|
||||||
|
var result2 = await _server.ReceiveAsync(_cts.Token);
|
||||||
|
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
|
||||||
|
|
||||||
|
await client.StopMotionAsync(_cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void StartMotion_BeforeConnect_Throws()
|
||||||
|
{
|
||||||
|
using var client = new FanucJ519Client();
|
||||||
|
Assert.Throws<InvalidOperationException>(() => client.StartMotion());
|
||||||
|
}
|
||||||
|
}
|
||||||
116
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
Normal file
116
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
Normal file
@@ -0,0 +1,116 @@
|
|||||||
|
using System.Buffers.Binary;
|
||||||
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 FANUC 真机三条通信链路的二进制协议基础与逆向抓包样本一致。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucProtocolTests
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 TCP 10012 程序命令封包与抓包中的 StopProg("RVBUSTSM") 完全一致。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void CommandProtocol_PacksCapturedStopProgramFrame()
|
||||||
|
{
|
||||||
|
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM");
|
||||||
|
|
||||||
|
Assert.Equal(
|
||||||
|
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
|
||||||
|
frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 TCP 10012 短响应和程序状态响应可以按抓包字段解析。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void CommandProtocol_ParsesCapturedResponses()
|
||||||
|
{
|
||||||
|
var stopResponse = FanucCommandProtocol.ParseResultResponse(
|
||||||
|
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
|
||||||
|
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
|
||||||
|
Convert.FromHexString("646f7a000000160000200300000000000000017a6f64"));
|
||||||
|
|
||||||
|
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
|
||||||
|
Assert.True(stopResponse.IsSuccess);
|
||||||
|
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, statusResponse.MessageId);
|
||||||
|
Assert.True(statusResponse.IsSuccess);
|
||||||
|
Assert.Equal(1u, statusResponse.ProgramStatus);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void StateProtocol_ParsesCapturedStateFrame()
|
||||||
|
{
|
||||||
|
var frame = FanucStateProtocol.ParseFrame(Convert.FromHexString(
|
||||||
|
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64"));
|
||||||
|
|
||||||
|
Assert.Equal(0u, frame.MessageId);
|
||||||
|
Assert.Equal(6, frame.Pose.Count);
|
||||||
|
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
||||||
|
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 UDP 60015 的 J519 初始化、结束和命令包字段布局。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void J519Protocol_PacksControlAndCommandPackets()
|
||||||
|
{
|
||||||
|
var command = new FanucJ519Command(
|
||||||
|
sequence: 2,
|
||||||
|
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||||||
|
|
||||||
|
var packet = FanucJ519Protocol.PackCommandPacket(command);
|
||||||
|
|
||||||
|
Assert.Equal(Convert.FromHexString("0000000000000001"), FanucJ519Protocol.PackInitPacket());
|
||||||
|
Assert.Equal(Convert.FromHexString("0000000200000001"), FanucJ519Protocol.PackEndPacket());
|
||||||
|
Assert.Equal(FanucJ519Protocol.CommandPacketLength, packet.Length);
|
||||||
|
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x00, 4)));
|
||||||
|
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x04, 4)));
|
||||||
|
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
|
||||||
|
Assert.Equal(2, packet[0x0d]);
|
||||||
|
Assert.Equal(1, packet[0x12]);
|
||||||
|
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
|
||||||
|
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
|
||||||
|
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void J519Protocol_ParsesResponsePacket()
|
||||||
|
{
|
||||||
|
var packet = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, 4), 0);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, 4), 1);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, 4), 12);
|
||||||
|
packet[0x0c] = 15;
|
||||||
|
packet[0x0d] = 2;
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, 2), 1);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, 2), 255);
|
||||||
|
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x12, 2), 10);
|
||||||
|
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x14, 4), 1234);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x18, 4), 100.5f);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x3c, 4), 1.25f);
|
||||||
|
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x60, 4), 2.5f);
|
||||||
|
|
||||||
|
var response = FanucJ519Protocol.ParseResponse(packet);
|
||||||
|
|
||||||
|
Assert.Equal(12u, response.Sequence);
|
||||||
|
Assert.Equal(15, response.Status);
|
||||||
|
Assert.True(response.AcceptsCommand);
|
||||||
|
Assert.True(response.ReceivedCommand);
|
||||||
|
Assert.True(response.SystemReady);
|
||||||
|
Assert.True(response.RobotInMotion);
|
||||||
|
Assert.Equal(10, response.ReadIoValue);
|
||||||
|
Assert.Equal(1234u, response.Timestamp);
|
||||||
|
Assert.Equal(100.5, response.Pose[0], precision: 6);
|
||||||
|
Assert.Equal(1.25, response.JointDegrees[0], precision: 6);
|
||||||
|
Assert.Equal(2.5, response.MotorCurrents[0], precision: 6);
|
||||||
|
}
|
||||||
|
}
|
||||||
138
tests/Flyshot.Core.Tests/FanucStateClientTests.cs
Normal file
138
tests/Flyshot.Core.Tests/FanucStateClientTests.cs
Normal file
@@ -0,0 +1,138 @@
|
|||||||
|
using System.Net;
|
||||||
|
using System.Net.Sockets;
|
||||||
|
using Flyshot.Runtime.Fanuc.Protocol;
|
||||||
|
|
||||||
|
namespace Flyshot.Core.Tests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 FANUC TCP 10010 状态通道客户端的后台接收与缓存能力。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class FanucStateClientTests : IDisposable
|
||||||
|
{
|
||||||
|
private readonly TcpListener _listener;
|
||||||
|
private readonly CancellationTokenSource _cts = new();
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 在随机可用端口启动本地模拟控制器。
|
||||||
|
/// </summary>
|
||||||
|
public FanucStateClientTests()
|
||||||
|
{
|
||||||
|
_listener = new TcpListener(IPAddress.Loopback, 0);
|
||||||
|
_listener.Start();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取分配给本地模拟控制器的端口。
|
||||||
|
/// </summary>
|
||||||
|
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 清理模拟控制器和取消源。
|
||||||
|
/// </summary>
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
_cts.Cancel();
|
||||||
|
_listener.Stop();
|
||||||
|
_cts.Dispose();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态客户端可以连接本地模拟控制器。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task ConnectAsync_ConnectsToLocalListener()
|
||||||
|
{
|
||||||
|
using var client = new FanucStateClient();
|
||||||
|
var acceptTask = _listener.AcceptTcpClientAsync();
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
Assert.True(client.IsConnected);
|
||||||
|
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证后台循环能正确解析抓包样本状态帧。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task GetLatestFrame_ReceivesAndParsesCapturedStateFrame()
|
||||||
|
{
|
||||||
|
using var client = new FanucStateClient();
|
||||||
|
var capturedFrame = Convert.FromHexString(
|
||||||
|
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||||
|
|
||||||
|
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
|
||||||
|
|
||||||
|
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||||
|
|
||||||
|
// 给后台循环留出接收和解析的时间。
|
||||||
|
await Task.Delay(200, _cts.Token);
|
||||||
|
|
||||||
|
var latest = client.GetLatestFrame();
|
||||||
|
Assert.NotNull(latest);
|
||||||
|
Assert.Equal(0u, latest.MessageId);
|
||||||
|
Assert.Equal(6, latest.Pose.Count);
|
||||||
|
Assert.Equal(9, latest.JointOrExtensionValues.Count);
|
||||||
|
Assert.Equal([2u, 0u, 0u, 1u], latest.TailWords);
|
||||||
|
|
||||||
|
client.Disconnect();
|
||||||
|
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态客户端在连接前调用 GetLatestFrame 返回 null。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public void GetLatestFrame_BeforeConnect_ReturnsNull()
|
||||||
|
{
|
||||||
|
using var client = new FanucStateClient();
|
||||||
|
Assert.Null(client.GetLatestFrame());
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证 Disconnect 后最新帧被清空。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task Disconnect_ClearsLatestFrame()
|
||||||
|
{
|
||||||
|
using var client = new FanucStateClient();
|
||||||
|
var capturedFrame = 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 启动模拟控制器,持续发送状态帧流。
|
||||||
|
/// </summary>
|
||||||
|
private async Task RunStreamingControllerAsync(byte[] frames, CancellationToken cancellationToken)
|
||||||
|
{
|
||||||
|
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||||
|
await using var stream = controller.GetStream();
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
while (!cancellationToken.IsCancellationRequested)
|
||||||
|
{
|
||||||
|
await stream.WriteAsync(frames, cancellationToken);
|
||||||
|
await Task.Delay(50, cancellationToken);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (OperationCanceledException)
|
||||||
|
{
|
||||||
|
// 正常取消。
|
||||||
|
}
|
||||||
|
catch (IOException)
|
||||||
|
{
|
||||||
|
// 客户端断开。
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -19,7 +19,7 @@ public sealed class RuntimeOrchestrationTests
|
|||||||
var runtime = new FanucControllerRuntime();
|
var runtime = new FanucControllerRuntime();
|
||||||
var robot = TestRobotFactory.CreateRobotProfile();
|
var robot = TestRobotFactory.CreateRobotProfile();
|
||||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||||
runtime.SetActiveController(sim: false);
|
runtime.SetActiveController(sim: true);
|
||||||
runtime.Connect("192.168.10.101");
|
runtime.Connect("192.168.10.101");
|
||||||
runtime.EnableRobot(bufferSize: 2);
|
runtime.EnableRobot(bufferSize: 2);
|
||||||
|
|
||||||
@@ -90,7 +90,7 @@ public sealed class RuntimeOrchestrationTests
|
|||||||
{
|
{
|
||||||
var service = TestRobotFactory.CreateCompatService();
|
var service = TestRobotFactory.CreateCompatService();
|
||||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||||
service.SetActiveController(sim: false);
|
service.SetActiveController(sim: true);
|
||||||
service.Connect("192.168.10.101");
|
service.Connect("192.168.10.101");
|
||||||
service.EnableRobot(2);
|
service.EnableRobot(2);
|
||||||
|
|
||||||
|
|||||||
@@ -53,7 +53,7 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
Assert.True(isSetupJson.RootElement.GetProperty("is_setup").GetBoolean());
|
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);
|
Assert.Equal(HttpStatusCode.OK, activeControllerResponse.StatusCode);
|
||||||
using var activeControllerJson = await ReadJsonAsync(activeControllerResponse);
|
using var activeControllerJson = await ReadJsonAsync(activeControllerResponse);
|
||||||
@@ -145,6 +145,24 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
Assert.Equal(HttpStatusCode.OK, getPoseResponse.StatusCode);
|
Assert.Equal(HttpStatusCode.OK, getPoseResponse.StatusCode);
|
||||||
using var getPoseJson = await ReadJsonAsync(getPoseResponse);
|
using var getPoseJson = await ReadJsonAsync(getPoseResponse);
|
||||||
Assert.Equal(7, getPoseJson.RootElement.GetProperty("pose").GetArrayLength());
|
Assert.Equal(7, getPoseJson.RootElement.GetProperty("pose").GetArrayLength());
|
||||||
|
|
||||||
|
using (var executeTrajectoryResponse = await client.PostAsJsonAsync("/execute_trajectory/", new
|
||||||
|
{
|
||||||
|
method = "icsp",
|
||||||
|
save_traj = true,
|
||||||
|
waypoints = new[]
|
||||||
|
{
|
||||||
|
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.1, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.2, 0.0, 0.0, 0.0, 0.0, 0.0 },
|
||||||
|
new[] { 0.3, 0.0, 0.0, 0.0, 0.0, 0.0 }
|
||||||
|
}
|
||||||
|
}))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, executeTrajectoryResponse.StatusCode);
|
||||||
|
using var executeTrajectoryJson = await ReadJsonAsync(executeTrajectoryResponse);
|
||||||
|
Assert.Equal("trajectory executed", executeTrajectoryJson.RootElement.GetProperty("status").GetString());
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
@@ -161,15 +179,19 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
addrs = new[]
|
addrs = new[]
|
||||||
{
|
{
|
||||||
new[] { 7, 8 },
|
new[] { 7, 8 },
|
||||||
new[] { 7, 8 }
|
new[] { 7, 8 },
|
||||||
|
Array.Empty<int>(),
|
||||||
|
Array.Empty<int>()
|
||||||
},
|
},
|
||||||
name = "demo-http-flyshot",
|
name = "demo-http-flyshot",
|
||||||
offset_values = new[] { 0.0, 1.0 },
|
offset_values = new[] { 0.0, 1.0, 0.0, 0.0 },
|
||||||
shot_flags = new[] { false, true },
|
shot_flags = new[] { false, true, false, false },
|
||||||
waypoints = new[]
|
waypoints = new[]
|
||||||
{
|
{
|
||||||
new[] { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
|
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);
|
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);
|
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);
|
||||||
using var executeJson = await ReadJsonAsync(executeResponse);
|
using var executeJson = await ReadJsonAsync(executeResponse);
|
||||||
@@ -197,6 +239,17 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
Assert.True(executeRoot.GetProperty("success").GetBoolean());
|
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" }))
|
using (var deleteResponse = await client.PostAsJsonAsync("/delete_flyshot/", new { name = "demo-http-flyshot" }))
|
||||||
{
|
{
|
||||||
Assert.Equal(HttpStatusCode.OK, deleteResponse.StatusCode);
|
Assert.Equal(HttpStatusCode.OK, deleteResponse.StatusCode);
|
||||||
@@ -215,7 +268,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
|
|||||||
server_ip = "127.0.0.1",
|
server_ip = "127.0.0.1",
|
||||||
port = 50001,
|
port = 50001,
|
||||||
robot_name = "FANUC_LR_Mate_200iD",
|
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);
|
Assert.Equal(HttpStatusCode.OK, initResponse.StatusCode);
|
||||||
|
|||||||
91
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
91
tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs
Normal file
@@ -0,0 +1,91 @@
|
|||||||
|
using System.Net;
|
||||||
|
using System.Net.Http.Json;
|
||||||
|
using System.Text.Json;
|
||||||
|
|
||||||
|
namespace Flyshot.Server.IntegrationTests;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态监控页面和状态快照 API 能读取当前 ControllerClient 兼容层状态。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFixture<FlyshotServerFactory>
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态页返回可由浏览器直接打开的 HTML,并引用状态快照 API。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task GetStatusPage_ReturnsMonitoringHtml()
|
||||||
|
{
|
||||||
|
using var client = factory.CreateClient();
|
||||||
|
|
||||||
|
using var response = await client.GetAsync("/status");
|
||||||
|
|
||||||
|
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||||
|
Assert.StartsWith("text/html", response.Content.Headers.ContentType?.MediaType);
|
||||||
|
|
||||||
|
var html = await response.Content.ReadAsStringAsync();
|
||||||
|
Assert.Contains("Flyshot Replacement 状态监控", html, StringComparison.Ordinal);
|
||||||
|
Assert.Contains("/api/status/snapshot", html, StringComparison.Ordinal);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。
|
||||||
|
/// </summary>
|
||||||
|
[Fact]
|
||||||
|
public async Task GetStatusSnapshot_ReturnsRuntimeStateAfterLegacyInitialization()
|
||||||
|
{
|
||||||
|
using var client = factory.CreateClient();
|
||||||
|
await InitializeRobotAsync(client);
|
||||||
|
|
||||||
|
using (var speedResponse = await client.PostAsJsonAsync("/set_speedRatio/", new { speed = 0.75 }))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, speedResponse.StatusCode);
|
||||||
|
}
|
||||||
|
|
||||||
|
using var response = await client.GetAsync("/api/status/snapshot");
|
||||||
|
|
||||||
|
Assert.Equal(HttpStatusCode.OK, response.StatusCode);
|
||||||
|
|
||||||
|
await using var responseStream = await response.Content.ReadAsStreamAsync();
|
||||||
|
using var json = await JsonDocument.ParseAsync(responseStream);
|
||||||
|
var root = json.RootElement;
|
||||||
|
var snapshot = root.GetProperty("snapshot");
|
||||||
|
|
||||||
|
Assert.Equal("ok", root.GetProperty("status").GetString());
|
||||||
|
Assert.True(root.GetProperty("isSetup").GetBoolean());
|
||||||
|
Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString());
|
||||||
|
Assert.Equal(6, root.GetProperty("degreesOfFreedom").GetInt32());
|
||||||
|
Assert.Empty(root.GetProperty("uploadedTrajectories").EnumerateArray());
|
||||||
|
Assert.Equal("Connected", snapshot.GetProperty("connectionState").GetString());
|
||||||
|
Assert.True(snapshot.GetProperty("isEnabled").GetBoolean());
|
||||||
|
Assert.False(snapshot.GetProperty("isInMotion").GetBoolean());
|
||||||
|
Assert.Equal(0.75, snapshot.GetProperty("speedRatio").GetDouble(), precision: 6);
|
||||||
|
Assert.Equal(6, snapshot.GetProperty("jointPositions").GetArrayLength());
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 初始化旧 HTTP 兼容链路,使状态页可以读取一个完整的已连接状态。
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="client">测试 HTTP 客户端。</param>
|
||||||
|
private static async Task InitializeRobotAsync(HttpClient client)
|
||||||
|
{
|
||||||
|
using (var setupResponse = await client.PostAsync("/setup_robot/?robot_name=FANUC_LR_Mate_200iD", content: null))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, setupResponse.StatusCode);
|
||||||
|
}
|
||||||
|
|
||||||
|
using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=true", content: null))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, activeControllerResponse.StatusCode);
|
||||||
|
}
|
||||||
|
|
||||||
|
using (var connectRobotResponse = await client.PostAsync("/connect_robot/?ip=192.168.10.101", content: null))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, connectRobotResponse.StatusCode);
|
||||||
|
}
|
||||||
|
|
||||||
|
using (var enableRobotResponse = await client.GetAsync("/enable_robot/"))
|
||||||
|
{
|
||||||
|
Assert.Equal(HttpStatusCode.OK, enableRobotResponse.StatusCode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user