feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码

* 扩展 ControllerClient 兼容层的执行参数和运行时编排
  * 新增 /status 页面与 /api/status/snapshot 状态快照接口
  * 补充 FANUC 协议、客户端和状态接口的最小验证测试
  * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
2026-04-24 21:26:25 +08:00
parent 8a20d9f507
commit a78e6761cb
25 changed files with 3773 additions and 55 deletions

View File

@@ -14,7 +14,7 @@
- 这是长期运行的无头后台服务,不是 GUI 桌面程序。
- 第一版仅面向当前现场组合,后续再扩展机型与控制柜适配。
- 当前仓库内已经移除宿主中的 `50001/TCP+JSON` 监听实现;现阶段只保留 ASP.NET Core HTTP 控制器层,以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。
- `ExecuteTrajectory``ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 最小链路;当前 `Flyshot.Runtime.Fanuc` 仍是状态型骨架,尚未接通真实 `10010 / 10012 / 60015` 通讯
- `ExecuteTrajectory``ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 最小链路;Web 状态页已通过 `/status``/api/status/snapshot` 暴露当前兼容层与运行时状态;`Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码,但运行时仍是状态型骨架,尚未完成真机 Socket 联调
- `50001/TCP+JSON` 的真实兼容入口如果后续需要恢复,必须基于 `docs/controller-client-api-compatibility-requirements.md``docs/controller-client-api-reverse-engineering.md` 重新评估,而不是直接把旧的 TCP 网关方向接回宿主。
开发约定:
@@ -32,5 +32,6 @@
- [x] 落地配置兼容与机器人模型解析
- [x] 落地轨迹规划与飞拍触发时间轴
- [x]`ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入最小 FANUC 运行时骨架
- [ ] 落地真实 `10010 / 10012 / 60015` FANUC 通讯
- [ ] 落地 Web 状态页
- [x] 落地 Web 状态页
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码
- [ ] 落地真实 `10010 / 10012 / 60015` FANUC Socket 通讯与现场联调

View File

@@ -131,5 +131,10 @@
- `Flyshot.ControllerClientCompat` 继续作为 HTTP 控制器后端兼容服务,不启动 `50001/TCP+JSON` 监听。
- `ExecuteTrajectory` 会先通过 `ICspPlanner` 规划普通轨迹,再把 `TrajectoryResult` 和最终关节位置交给 `IControllerRuntime`
- `ExecuteFlyShotTraj` 会从上传轨迹目录取出轨迹,通过 `SelfAdaptIcspPlanner` 规划并用 `ShotTimelineBuilder` 生成 `ShotEvent` / `TrajectoryDoEvent`
- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态真实 `10010 / 10012 / 60015` Socket 通讯尚未落地
- HTTP 控制器已经按公开文档补齐 `ExecuteTrajectory(method, save_traj)``ExecuteFlyShotTraj(move_to_start, method, save_traj, use_cache)` 参数,并继续兼容旧的裸 waypoint 数组和只传 `name` 的请求体
- `method="icsp"``method="self-adapt-icsp"` 已接入当前规划器;`method="doubles"` 会被识别但返回显式未实现,不会静默降级成 ICSP。
- `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码,并使用逆向抓包样本覆盖最小测试。
- `Flyshot.Runtime.Fanuc` 当前只保存连接、使能、速度、IO、TCP、关节位置和执行结果状态真实 `10010 / 10012 / 60015` Socket 通讯与现场联调尚未落地。
- 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`,用于查看兼容层初始化、机器人元数据和运行时快照。
- `MoveJoint` 仍保持旧兼容语义中的直接运动接口,但状态写入已经统一经过运行时,而不是由兼容服务自己维护关节数组。
- `GetNearestIK``SetUpRobotFromEnv` 当前已经暴露完整参数形状,但后端求解器 / 环境文件解析仍返回显式未实现。

View 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 10012Req/Res 同步命令通道)
FanucStateClient (TCP 10010持续接收状态帧后台循环)
FanucJ519Client (UDP 600158ms 周期发送 + 接收响应)
↓ 使用现有编解码
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 个核心测试仍然通过

View File

@@ -261,8 +261,9 @@ Expected: PASS, with existing HTTP compatibility tests still green.
```markdown
- [x] 落地最小 FANUC 运行时骨架
- [x] 将 ExecuteTrajectory / ExecuteFlyShotTraj 接入 Planning + Triggering + Runtime
- [ ] 落地真实 10010 / 10012 / 60015 通讯
- [ ] 落地 Web 状态页
- [x] 落地 Web 状态页
- [x] 固化 10010 / 10012 / 60015 FANUC 基础协议帧编解码
- [ ] 落地真实 10010 / 10012 / 60015 Socket 通讯与现场联调
```
- [x] **Step 2: Run final build**

View File

@@ -18,6 +18,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
private string? _configuredRobotName;
private string? _connectedServerIp;
private int _connectedServerPort;
private bool _showTcp = true;
private double _showTcpAxisLength = 0.1;
private int _showTcpAxisSize = 2;
/// <summary>
/// 初始化一份 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 />
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 />
public void SetActiveController(bool sim)
{
@@ -159,6 +209,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
}
/// <inheritdoc />
public ControllerStateSnapshot GetControllerSnapshot()
{
return _runtime.GetSnapshot();
}
/// <inheritdoc />
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 />
public void SetTcp(double x, double y, double z)
{
@@ -242,9 +321,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
/// <inheritdoc />
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints)
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
{
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
if (waypoints.Count == 0)
{
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
@@ -255,8 +335,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
var robot = RequireActiveRobot();
EnsureRuntimeEnabled();
// 普通轨迹必须先通过 ICSP 规划,再把规划结果交给运行时执行。
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints);
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options);
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
}
@@ -294,8 +374,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
/// <inheritdoc />
public void ExecuteTrajectoryByName(string name)
public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
{
options ??= new FlyshotExecutionOptions();
if (string.IsNullOrWhiteSpace(name))
{
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
@@ -316,13 +397,68 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
}
// 已上传飞拍轨迹必须生成 shot timeline 后再交给运行时。
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory);
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options);
if (options.MoveToStart)
{
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions);
}
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
}
}
/// <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 />
public void DeleteTrajectory(string name)
{

View File

@@ -12,6 +12,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
private readonly ICspPlanner _icspPlanner = new();
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new();
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
/// <summary>
/// 对普通轨迹执行 ICSP 规划。
@@ -21,10 +22,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints)
IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions();
var program = CreateProgram(
name: "ordinary-trajectory",
@@ -33,14 +36,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: Enumerable.Repeat(0, waypoints.Count),
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
var method = ParseOrdinaryMethod(options.Method);
var request = new TrajectoryRequest(
robot: robot,
program: program,
method: PlanningMethod.Icsp);
method: method,
saveTrajectoryArtifacts: options.SaveTrajectory);
var plannedTrajectory = _icspPlanner.Plan(request);
var plannedTrajectory = PlanByMethod(request, method);
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
var result = CreateResult(plannedTrajectory, shotTimeline);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
}
@@ -51,10 +56,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="robot">当前机器人配置。</param>
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded)
public PlannedExecutionBundle PlanUploadedFlyshot(
RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null)
{
ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions();
var program = CreateProgram(
name: uploaded.Name,
@@ -63,19 +72,152 @@ public sealed class ControllerClientTrajectoryOrchestrator
offsetValues: uploaded.OffsetValues,
addressGroups: uploaded.AddressGroups);
var method = ParseFlyshotMethod(options.Method);
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options);
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志,规划轨迹和触发时间轴保持不可变复用。
return new PlannedExecutionBundle(
cachedBundle.PlannedTrajectory,
cachedBundle.ShotTimeline,
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
}
var request = new TrajectoryRequest(
robot: robot,
program: program,
method: PlanningMethod.SelfAdaptIcsp);
method: method,
moveToStart: options.MoveToStart,
saveTrajectoryArtifacts: options.SaveTrajectory,
useCache: options.UseCache);
var plannedTrajectory = _selfAdaptIcspPlanner.Plan(request);
var plannedTrajectory = PlanByMethod(request, method);
var shotTimeline = _shotTimelineBuilder.Build(
plannedTrajectory,
holdCycles: 0,
samplePeriod: robot.ServoPeriod);
var result = CreateResult(plannedTrajectory, shotTimeline);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
if (options.UseCache)
{
_flyshotCache[cacheKey] = bundle;
}
return bundle;
}
/// <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>
@@ -108,7 +250,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
/// <param name="plannedTrajectory">规划后的轨迹。</param>
/// <param name="shotTimeline">触发时间轴。</param>
/// <returns>运行时执行结果描述。</returns>
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline)
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
{
return new TrajectoryResult(
programName: plannedTrajectory.OriginalProgram.Name,
@@ -119,7 +261,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
triggerTimeline: shotTimeline.TriggerTimeline,
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
usedCache: usedCache,
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount);
}

View File

@@ -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; }
}

View File

@@ -1,3 +1,5 @@
using Flyshot.Core.Domain;
namespace Flyshot.ControllerClientCompat;
/// <summary>
@@ -22,12 +24,38 @@ public interface IControllerClientCompatService
/// <param name="port">客户端传入的服务端端口。</param>
void ConnectServer(string serverIp, int port);
/// <summary>
/// 获取兼容服务端版本号。
/// </summary>
/// <returns>服务端版本号。</returns>
string GetServerVersion();
/// <summary>
/// 获取兼容客户端版本号。
/// </summary>
/// <returns>客户端版本号。</returns>
string GetClientVersion();
/// <summary>
/// 根据旧客户端使用的机器人名称完成机器人初始化。
/// </summary>
/// <param name="robotName">机器人名称。</param>
void SetUpRobot(string robotName);
/// <summary>
/// 根据旧客户端传入的环境文件完成机器人初始化。
/// </summary>
/// <param name="envFile">环境文件路径。</param>
void SetUpRobotFromEnv(string envFile);
/// <summary>
/// 设置是否显示 TCP 坐标轴。
/// </summary>
/// <param name="isShow">是否显示 TCP。</param>
/// <param name="axisLength">坐标轴长度。</param>
/// <param name="axisSize">坐标轴线宽。</param>
void SetShowTcp(bool isShow, double axisLength, int axisSize);
/// <summary>
/// 记录当前激活的控制器类型。
/// </summary>
@@ -61,6 +89,12 @@ public interface IControllerClientCompatService
/// </summary>
void StopMove();
/// <summary>
/// 读取当前控制器运行时状态快照。
/// </summary>
/// <returns>控制器运行时状态快照。</returns>
ControllerStateSnapshot GetControllerSnapshot();
/// <summary>
/// 获取当前速度倍率。
/// </summary>
@@ -89,6 +123,14 @@ public interface IControllerClientCompatService
/// <returns>缓存中的 IO 值。</returns>
bool GetIo(int port, string ioType);
/// <summary>
/// 按给定位姿和 seed 计算最近 IK。
/// </summary>
/// <param name="pose">目标位姿数组。</param>
/// <param name="seed">IK seed 关节数组。</param>
/// <returns>IK 结果关节数组。</returns>
IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed);
/// <summary>
/// 设置当前 TCP 三维坐标。
/// </summary>
@@ -119,7 +161,8 @@ public interface IControllerClientCompatService
/// 执行普通轨迹。
/// </summary>
/// <param name="waypoints">轨迹路点集合。</param>
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints);
/// <param name="options">执行参数。</param>
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null);
/// <summary>
/// 读取当前末端位姿快照。
@@ -143,7 +186,25 @@ public interface IControllerClientCompatService
/// 执行指定名称的飞拍轨迹。
/// </summary>
/// <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>
/// 删除指定名称的飞拍轨迹。

View File

@@ -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; }
}

View File

@@ -1,15 +1,21 @@
using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Runtime.Fanuc;
/// <summary>
/// 提供第一阶段 FANUC 控制器运行时骨架集中保存连接、使能、IO 和运动结果状态
/// FANUC 控制器运行时,将上层兼容层指令转换为三条真实 Socket 通道的交互
/// 仿真模式下仍保持内存桩行为,便于离线测试与回退。
/// </summary>
public sealed class FanucControllerRuntime : IControllerRuntime
public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
{
private readonly object _stateLock = new();
private readonly Dictionary<(string IoType, int Port), bool> _ioValues = new();
private readonly FanucCommandClient _commandClient = new();
private readonly FanucStateClient _stateClient = new();
private readonly FanucJ519Client _j519Client = new();
private RobotProfile? _robot;
private string? _robotName;
private bool? _activeControllerIsSimulation;
@@ -21,6 +27,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
private double[] _tcp = [0.0, 0.0, 0.0];
private double[] _jointPositions = Array.Empty<double>();
private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0];
private bool _disposed;
/// <inheritdoc />
public void ResetRobot(RobotProfile robot, string robotName)
@@ -33,7 +40,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
// 重新初始化机器人时清空底层控制器状态,匹配旧 ControllerClient 的初始化顺序。
DisconnectClients();
_robot = robot;
_robotName = robotName;
_activeControllerIsSimulation = null;
@@ -55,6 +62,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
DisconnectClients();
_activeControllerIsSimulation = sim;
_connectedRobotIp = null;
_isEnabled = false;
@@ -73,6 +81,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureActiveControllerSelected();
if (_activeControllerIsSimulation!.Value)
{
_connectedRobotIp = robotIp;
_isEnabled = false;
_isInMotion = false;
return;
}
// 真机模式:顺序建立三条通道 — 状态 → 命令 → 运动。
_stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult();
_commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult();
_j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult();
_connectedRobotIp = robotIp;
_isEnabled = false;
_isInMotion = false;
@@ -85,6 +106,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
DisconnectClients();
_connectedRobotIp = null;
_isEnabled = false;
_isInMotion = false;
@@ -103,6 +125,20 @@ public sealed class FanucControllerRuntime : IControllerRuntime
{
EnsureConnected();
_bufferSize = bufferSize;
if (IsSimulationMode)
{
_isEnabled = true;
return;
}
// 真机模式:走完整 RVBUSTSM 启动序列(与抓包一致)。
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
_commandClient.ResetRobotAsync().GetAwaiter().GetResult();
_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult();
_commandClient.StartProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
_j519Client.StartMotion();
_isEnabled = true;
}
}
@@ -113,6 +149,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
{
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
_commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult();
}
_isEnabled = false;
_isInMotion = false;
}
@@ -124,6 +166,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
{
_j519Client.StopMotionAsync().GetAwaiter().GetResult();
}
_isInMotion = false;
}
}
@@ -213,6 +260,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
{
var frame = _stateClient.GetLatestFrame();
if (frame?.JointOrExtensionValues.Count >= _jointPositions.Length)
{
return frame.JointOrExtensionValues.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
}
}
return _jointPositions.ToArray();
}
}
@@ -223,6 +279,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime
lock (_stateLock)
{
EnsureRobotSetup();
if (!IsSimulationMode)
{
var frame = _stateClient.GetLatestFrame();
if (frame?.Pose.Count >= 6)
{
return frame.Pose.Take(6).Select(v => (double)v).ToArray();
}
}
return _pose.ToArray();
}
}
@@ -232,14 +297,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime
{
lock (_stateLock)
{
var jointPositions = _jointPositions;
var cartesianPose = _pose;
var isInMotion = _isInMotion;
if (!IsSimulationMode)
{
var frame = _stateClient.GetLatestFrame();
if (frame is not null)
{
if (frame.JointOrExtensionValues.Count >= jointPositions.Length)
{
jointPositions = frame.JointOrExtensionValues.Take(jointPositions.Length).Select(v => (double)v).ToArray();
}
if (frame.Pose.Count >= 6)
{
cartesianPose = frame.Pose.Take(6).Select(v => (double)v).ToArray();
}
}
var j519Response = _j519Client.GetLatestResponse();
if (j519Response is not null)
{
isInMotion = j519Response.RobotInMotion;
}
}
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: ResolveConnectionState(),
isEnabled: _isEnabled,
isInMotion: _isInMotion,
isInMotion: isInMotion,
speedRatio: _speedRatio,
jointPositions: _jointPositions,
cartesianPose: _pose,
jointPositions: jointPositions,
cartesianPose: cartesianPose,
activeAlarms: Array.Empty<RuntimeAlarm>());
}
}
@@ -256,18 +348,68 @@ public sealed class FanucControllerRuntime : IControllerRuntime
EnsureValidTrajectory(result);
EnsureJointCount(finalJointPositions.Count);
// 第一阶段没有真机 Socket 流,先把执行结果收敛到统一运行时状态。
if (!IsSimulationMode)
{
// 真机模式:通过 J519 发送最终关节目标。
// TODO: 后续接入稠密路点流,当前先发送单点收敛。
var command = new FanucJ519Command(
sequence: 0,
targetJoints: finalJointPositions.Select(j => (double)j).ToArray());
_j519Client.UpdateCommand(command);
}
_isInMotion = true;
_jointPositions = finalJointPositions.ToArray();
_isInMotion = false;
}
}
/// <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>
/// 归一化 IO 类型字符串,避免调用方大小写差异影响缓存键。
/// </summary>
/// <param name="ioType">调用方传入的 IO 类型。</param>
/// <returns>标准化后的 IO 类型。</returns>
private static string NormalizeIoType(string ioType)
{
if (string.IsNullOrWhiteSpace(ioType))
@@ -281,7 +423,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
/// <summary>
/// 校验轨迹规划结果可执行。
/// </summary>
/// <param name="result">规划结果。</param>
private static void EnsureValidTrajectory(TrajectoryResult result)
{
if (!result.IsValid)
@@ -293,7 +434,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
/// <summary>
/// 根据当前内部状态生成连接状态标签。
/// </summary>
/// <returns>面向监控和测试的连接状态。</returns>
private string ResolveConnectionState()
{
if (_robot is null)
@@ -307,7 +447,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime
/// <summary>
/// 校验给定关节数组长度与当前机器人自由度一致。
/// </summary>
/// <param name="jointCount">调用方传入的关节数。</param>
private void EnsureJointCount(int jointCount)
{
var expectedJointCount = _robot?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");

View 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;
}
}
}

View 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);
}
}

View 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 客户端已释放,退出循环。
}
}
}

View 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;
}
}

View 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;
}
}
}

View 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 状态帧长度字段与实际长度不一致。");
}
}
}

View File

@@ -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>
/// 兼容旧 `/setup_robot/` 路由。
/// </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>
/// 兼容旧 `/is_setup/` 路由。
/// </summary>
@@ -81,15 +144,16 @@ public sealed class LegacyHttpApiController : ControllerBase
}
/// <summary>
/// 兼容旧 `/enable_robot/` 路由;保持原 Python 服务固定传 `8` 的行为
/// 兼容旧 `EnableRobot(buffer_size=2)` 参数形状
/// </summary>
/// <param name="buffer_size">控制器执行缓冲区大小。</param>
/// <returns>旧 FastAPI 层风格的布尔状态响应。</returns>
[HttpGet("/enable_robot/")]
public IActionResult EnableRobot()
public IActionResult EnableRobot([FromQuery] int buffer_size = 2)
{
try
{
_compatService.EnableRobot(8);
_compatService.EnableRobot(buffer_size);
return Ok(new { enable_robot = true });
}
catch
@@ -116,6 +180,24 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 提供与旧客户端 <c>StopMove</c> 语义对应的 HTTP 端点。
/// </summary>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpGet("/stop_move/")]
public IActionResult StopMove()
{
try
{
_compatService.StopMove();
return Ok(new { status = "move stopped" });
}
catch
{
return LegacyBadRequest("StopMove failed");
}
}
/// <summary>
/// 兼容旧 `/set_active_controller/` 路由。
/// </summary>
@@ -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>
/// 兼容旧 `/robot_info/` 路由。
/// </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>
/// 兼容旧 `/get_joint_position/` 路由。
/// </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>
/// 兼容旧 `/list_flyShotTraj/` 路由。
/// </summary>
@@ -292,11 +433,17 @@ public sealed class LegacyHttpApiController : ControllerBase
/// <param name="waypoints">轨迹请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/execute_trajectory/")]
public IActionResult ExecuteTrajectory([FromBody] JsonElement waypoints)
public IActionResult ExecuteTrajectory(
[FromBody] JsonElement waypoints,
[FromQuery] string? method = null,
[FromQuery] bool? save_traj = null)
{
try
{
_compatService.ExecuteTrajectory(ParseLegacyTrajectoryWaypoints(waypoints));
var request = ParseExecuteTrajectoryRequest(waypoints, method, save_traj);
_compatService.ExecuteTrajectory(
request.Waypoints,
new TrajectoryExecutionOptions(request.Method, request.SaveTrajectory));
return Ok(new { status = "trajectory executed" });
}
catch
@@ -349,14 +496,20 @@ public sealed class LegacyHttpApiController : ControllerBase
/// <summary>
/// 兼容旧 `/execute_flyshot/` 路由。
/// </summary>
/// <param name="data">包含轨迹名称的请求体。</param>
/// <param name="data">包含轨迹名称和执行参数的请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/execute_flyshot/")]
public IActionResult ExecuteFlyshot([FromBody] LegacyNameRequest data)
public IActionResult ExecuteFlyshot([FromBody] LegacyExecuteFlyshotRequest data)
{
try
{
_compatService.ExecuteTrajectoryByName(data.name);
_compatService.ExecuteTrajectoryByName(
data.name,
new FlyshotExecutionOptions(
moveToStart: data.move_to_start,
method: data.method,
saveTrajectory: data.save_traj,
useCache: data.use_cache));
return Ok(new { status = "FlyShot executed", success = true });
}
catch (Exception exception)
@@ -365,6 +518,57 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 兼容旧 `SaveTrajInfo(name, method)` 参数形状。
/// </summary>
/// <param name="request">轨迹保存请求体。</param>
/// <returns>旧 FastAPI 层风格的状态响应。</returns>
[HttpPost("/save_traj_info/")]
public IActionResult SaveTrajectoryInfo([FromBody] LegacyTrajectoryInfoRequest request)
{
try
{
_compatService.SaveTrajectoryInfo(request.name, request.method);
return Ok(new { status = "trajectory info saved", success = true });
}
catch (NotSupportedException exception)
{
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
}
catch
{
return LegacyBadRequest("SaveTrajInfo failed");
}
}
/// <summary>
/// 兼容旧 `IsFlyShotTrajValid(time, name, method, save_traj)` 参数形状。
/// </summary>
/// <param name="request">轨迹有效性检查请求体。</param>
/// <returns>有效性和轨迹时长。</returns>
[HttpPost("/is_flyShotTrajValid/")]
public IActionResult IsFlyshotTrajectoryValid([FromBody] LegacyFlyshotValidationRequest request)
{
try
{
var isValid = _compatService.IsFlyshotTrajectoryValid(
out var duration,
request.name,
request.method,
request.save_traj);
return Ok(new { success = isValid, valid = isValid, time = duration.TotalSeconds });
}
catch (NotSupportedException exception)
{
return StatusCode(StatusCodes.Status501NotImplemented, new { detail = exception.Message });
}
catch
{
return LegacyBadRequest("IsFlyShotTrajValid failed");
}
}
/// <summary>
/// 兼容旧 `/set_speedRatio/` 路由。
/// </summary>
@@ -420,7 +624,7 @@ public sealed class LegacyHttpApiController : ControllerBase
return LegacyBadRequest("Robot not setup");
}
_compatService.SetActiveController(sim: false);
_compatService.SetActiveController(data.sim);
_compatService.Connect(data.robot_ip);
_compatService.EnableRobot(2);
return Ok(new { message = "init_Success", returnCode = 0 });
@@ -448,6 +652,47 @@ public sealed class LegacyHttpApiController : ControllerBase
}
}
/// <summary>
/// 解析旧 `/execute_trajectory/` 的完整参数形状。
/// </summary>
/// <param name="payload">原始 JSON 请求体。</param>
/// <param name="queryMethod">查询字符串中的 method 覆盖值。</param>
/// <param name="querySaveTrajectory">查询字符串中的 save_traj 覆盖值。</param>
/// <returns>统一后的路点和执行参数。</returns>
private static (
IReadOnlyList<IReadOnlyList<double>> Waypoints,
string Method,
bool SaveTrajectory) ParseExecuteTrajectoryRequest(
JsonElement payload,
string? queryMethod,
bool? querySaveTrajectory)
{
string method = queryMethod ?? "icsp";
bool saveTrajectory = querySaveTrajectory ?? false;
if (payload.ValueKind == JsonValueKind.Object)
{
if (payload.TryGetProperty("method", out var methodElement) && methodElement.ValueKind == JsonValueKind.String)
{
method = methodElement.GetString() ?? method;
}
if (payload.TryGetProperty("save_traj", out var saveTrajectoryElement))
{
saveTrajectory = saveTrajectoryElement.GetBoolean();
}
if (!payload.TryGetProperty("waypoints", out var waypointElement))
{
throw new InvalidOperationException("ExecuteTrajectory request body must include waypoints.");
}
return (ParseLegacyTrajectoryWaypoints(waypointElement), method, saveTrajectory);
}
return (ParseLegacyTrajectoryWaypoints(payload), method, saveTrajectory);
}
/// <summary>
/// 解析旧 `/execute_trajectory/` 可能出现的两种历史请求体形状。
/// </summary>
@@ -576,6 +821,90 @@ public sealed class LegacyNameRequest
public string name { get; init; } = string.Empty;
}
/// <summary>
/// 表示旧 `/execute_flyshot/` 路由使用的完整执行请求体。
/// </summary>
public sealed class LegacyExecuteFlyshotRequest
{
/// <summary>
/// 获取或设置轨迹名称。
/// </summary>
public string name { get; init; } = string.Empty;
/// <summary>
/// 获取或设置是否先移动到轨迹起点。
/// </summary>
public bool move_to_start { get; init; } = true;
/// <summary>
/// 获取或设置轨迹生成方法。
/// </summary>
public string method { get; init; } = "icsp";
/// <summary>
/// 获取或设置是否保存轨迹信息。
/// </summary>
public bool save_traj { get; init; } = true;
/// <summary>
/// 获取或设置是否复用轨迹缓存。
/// </summary>
public bool use_cache { get; init; } = true;
}
/// <summary>
/// 表示旧 `SaveTrajInfo` 参数形状。
/// </summary>
public sealed class LegacyTrajectoryInfoRequest
{
/// <summary>
/// 获取或设置轨迹名称。
/// </summary>
public string name { get; init; } = string.Empty;
/// <summary>
/// 获取或设置轨迹生成方法。
/// </summary>
public string method { get; init; } = "icsp";
}
/// <summary>
/// 表示旧 `IsFlyShotTrajValid` 参数形状。
/// </summary>
public sealed class LegacyFlyshotValidationRequest
{
/// <summary>
/// 获取或设置轨迹名称。
/// </summary>
public string name { get; init; } = string.Empty;
/// <summary>
/// 获取或设置轨迹生成方法。
/// </summary>
public string method { get; init; } = "icsp";
/// <summary>
/// 获取或设置是否保存轨迹信息。
/// </summary>
public bool save_traj { get; init; } = true;
}
/// <summary>
/// 表示旧 `GetNearestIK` 参数形状。
/// </summary>
public sealed class LegacyNearestIkRequest
{
/// <summary>
/// 获取或设置目标位姿 `[x,y,z,qx,qy,qz,qw]`。
/// </summary>
public List<double> pose { get; init; } = [];
/// <summary>
/// 获取或设置 IK seed 关节数组。
/// </summary>
public List<double> seed { get; init; } = [];
}
/// <summary>
/// 表示旧 `/set_speedRatio/` 路由使用的速度倍率请求体。
/// </summary>
@@ -611,4 +940,9 @@ public sealed class LegacyInitMpcRobotRequest
/// 获取或设置机器人控制器 IP。
/// </summary>
public string robot_ip { get; init; } = string.Empty;
/// <summary>
/// 获取或设置是否使用仿真控制器;默认 false 连接真机。
/// </summary>
public bool sim { get; init; }
}

View 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
});
}
}

View 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;
}
}
}

View 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());
}
}

View 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);
}
}

View 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)
{
// 客户端断开。
}
}
}

View File

@@ -19,7 +19,7 @@ public sealed class RuntimeOrchestrationTests
var runtime = new FanucControllerRuntime();
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
runtime.SetActiveController(sim: false);
runtime.SetActiveController(sim: true);
runtime.Connect("192.168.10.101");
runtime.EnableRobot(bufferSize: 2);
@@ -90,7 +90,7 @@ public sealed class RuntimeOrchestrationTests
{
var service = TestRobotFactory.CreateCompatService();
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.SetActiveController(sim: true);
service.Connect("192.168.10.101");
service.EnableRobot(2);

View File

@@ -53,7 +53,7 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.True(isSetupJson.RootElement.GetProperty("is_setup").GetBoolean());
}
using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=false", content: null))
using (var activeControllerResponse = await client.PostAsync("/set_active_controller/?sim=true", content: null))
{
Assert.Equal(HttpStatusCode.OK, activeControllerResponse.StatusCode);
using var activeControllerJson = await ReadJsonAsync(activeControllerResponse);
@@ -145,6 +145,24 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.Equal(HttpStatusCode.OK, getPoseResponse.StatusCode);
using var getPoseJson = await ReadJsonAsync(getPoseResponse);
Assert.Equal(7, getPoseJson.RootElement.GetProperty("pose").GetArrayLength());
using (var executeTrajectoryResponse = await client.PostAsJsonAsync("/execute_trajectory/", new
{
method = "icsp",
save_traj = true,
waypoints = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.1, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.2, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.3, 0.0, 0.0, 0.0, 0.0, 0.0 }
}
}))
{
Assert.Equal(HttpStatusCode.OK, executeTrajectoryResponse.StatusCode);
using var executeTrajectoryJson = await ReadJsonAsync(executeTrajectoryResponse);
Assert.Equal("trajectory executed", executeTrajectoryJson.RootElement.GetProperty("status").GetString());
}
}
/// <summary>
@@ -161,15 +179,19 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
addrs = new[]
{
new[] { 7, 8 },
new[] { 7, 8 }
new[] { 7, 8 },
Array.Empty<int>(),
Array.Empty<int>()
},
name = "demo-http-flyshot",
offset_values = new[] { 0.0, 1.0 },
shot_flags = new[] { false, true },
offset_values = new[] { 0.0, 1.0, 0.0, 0.0 },
shot_flags = new[] { false, true, false, false },
waypoints = new[]
{
new[] { 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
new[] { 1.1, 1.2, 1.3, 1.4, 1.5, 1.6 }
new[] { 0.2, 0.2, 0.3, 0.4, 0.5, 0.6 },
new[] { 0.3, 0.2, 0.3, 0.4, 0.5, 0.6 },
new[] { 0.4, 0.2, 0.3, 0.4, 0.5, 0.6 }
}
};
@@ -188,7 +210,27 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.Contains("demo-http-flyshot", names);
}
using (var executeResponse = await client.PostAsJsonAsync("/execute_flyshot/", new { name = "demo-http-flyshot" }))
using (var validResponse = await client.PostAsJsonAsync("/is_flyShotTrajValid/", new
{
name = "demo-http-flyshot",
method = "icsp",
save_traj = false
}))
{
Assert.Equal(HttpStatusCode.OK, validResponse.StatusCode);
using var validJson = await ReadJsonAsync(validResponse);
Assert.True(validJson.RootElement.GetProperty("valid").GetBoolean());
Assert.True(validJson.RootElement.GetProperty("time").GetDouble() > 0.0);
}
using (var executeResponse = await client.PostAsJsonAsync("/execute_flyshot/", new
{
name = "demo-http-flyshot",
move_to_start = true,
method = "icsp",
save_traj = true,
use_cache = true
}))
{
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);
using var executeJson = await ReadJsonAsync(executeResponse);
@@ -197,6 +239,17 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
Assert.True(executeRoot.GetProperty("success").GetBoolean());
}
using (var saveInfoResponse = await client.PostAsJsonAsync("/save_traj_info/", new
{
name = "demo-http-flyshot",
method = "icsp"
}))
{
Assert.Equal(HttpStatusCode.OK, saveInfoResponse.StatusCode);
using var saveInfoJson = await ReadJsonAsync(saveInfoResponse);
Assert.True(saveInfoJson.RootElement.GetProperty("success").GetBoolean());
}
using (var deleteResponse = await client.PostAsJsonAsync("/delete_flyshot/", new { name = "demo-http-flyshot" }))
{
Assert.Equal(HttpStatusCode.OK, deleteResponse.StatusCode);
@@ -215,7 +268,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
server_ip = "127.0.0.1",
port = 50001,
robot_name = "FANUC_LR_Mate_200iD",
robot_ip = "192.168.10.101"
robot_ip = "192.168.10.101",
sim = true
});
Assert.Equal(HttpStatusCode.OK, initResponse.StatusCode);

View 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);
}
}
}