✨ feat(runtime): 完善 FANUC 命令参数与状态通道重连
* 在 FanucCommandProtocol/Client 中补齐速度倍率、TCP 位姿和 IO 的封包/解析，并引入 FanucIoTypes 字符串到枚举映射 * FanucControllerRuntime 在非仿真模式下接入真机命令通道，本地 缓存仅作为兜底，TCP 操作扩展为 7 维 Pose * FanucStateClient 增加帧超时检测、退避自动重连和诊断状态接口, 超时或重连期间不再把陈旧帧当作当前机器人状态 * FanucStateProtocol 锁定 90B 帧字段为 pose[6]、joint[6]、 external_axes[3] 和 raw_tail_words[4],并保留状态字诊断槽位 * ICspPlanner 增加 global_scale > 1.0 失败判定,self-adapt-icsp 内部禁用该判定以保留补点重试链路 * 同步更新 README/AGENTS/计划文档的 todo 状态和实现说明
This commit is contained in:
@@ -175,5 +175,6 @@ flyshot-replacement/
|
||||
- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp`，并已完成旧系统导出轨迹对齐。
|
||||
- `Flyshot.Core.Triggering` 已能从 `shot_flags / offset_values / addr` 生成触发时间轴。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 真机抓包确认为 90B。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端和 J519 周期发送链路，但速度倍率、TCP、IO、J519 闭环与现场联调仍需补齐。
|
||||
- `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。
|
||||
- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路，但 J519 闭环与现场联调仍需补齐。
|
||||
- `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。
|
||||
|
||||
18
README.md
18
README.md
@@ -18,7 +18,7 @@
|
||||
- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。
|
||||
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。
|
||||
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 真机抓包确认为 90B 固定帧。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停和 J519 周期发送能力，但速度倍率、TCP、IO、J519 闭环和现场联调仍需补齐。
|
||||
- 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送能力，但 J519 闭环和现场联调仍需补齐。
|
||||
|
||||
开发约定：
|
||||
|
||||
@@ -39,6 +39,7 @@
|
||||
- [x] 落地 Web 状态页
|
||||
- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码，确认 `10010` 状态帧为 90B
|
||||
- [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发
|
||||
- [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析
|
||||
- [x] 保留新 HTTP 接口路线，明确不再实现旧 `50001/TCP+JSON` 网关
|
||||
|
||||
剩余 Todo：
|
||||
@@ -49,21 +50,22 @@
|
||||
- [ ] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流。
|
||||
|
||||
2. 轨迹规划
|
||||
- [ ] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。
|
||||
- [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。
|
||||
- [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。
|
||||
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。
|
||||
|
||||
3. FANUC TCP 10012 命令通道
|
||||
- [ ] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
|
||||
- [ ] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。
|
||||
- [ ] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。
|
||||
- [x] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。
|
||||
- [x] 所有命令响应必须检查 `result_code`，失败时返回可诊断错误，而不是只更新本地缓存。
|
||||
|
||||
4. FANUC TCP 10010 状态通道
|
||||
- [ ] 用 `j519 协议.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [ ] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||
- [ ] 补充断线、异常帧、超时和重连策略。
|
||||
- [x] 用 `j519 协议.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。
|
||||
- [x] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。
|
||||
- [x] 补充断线清理和异常帧拒绝测试。
|
||||
- [x] 补充状态通道超时和重连策略，超时后标记陈旧状态并按退避策略自动重连。
|
||||
|
||||
5. FANUC UDP 60015 J519 运动链路
|
||||
- [ ] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界，避免双重节拍或命令覆盖。
|
||||
|
||||
@@ -5,6 +5,7 @@
|
||||
当前 `flyshot-replacement` 项目已完成：
|
||||
- 三条 FANUC 通信链路的二进制协议编解码（`FanucCommandProtocol`、`FanucStateProtocol`、`FanucJ519Protocol`）
|
||||
- 抓包样本验证的协议测试（5 个 FanucProtocolTests 全部通过）
|
||||
- TCP 10012 的 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 参数命令封包、响应解析和本地模拟器测试
|
||||
- HTTP 兼容层控制器和状态监控页
|
||||
- 轨迹规划与飞拍触发编排层
|
||||
|
||||
@@ -64,9 +65,9 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- `ResetRobotAsync()` → `PackEmptyCommand(0x2100)`
|
||||
- `GetProgramStatusAsync(string name)` → `PackProgramCommand(0x2003, name)`
|
||||
- `StartProgramAsync(string name)` → `PackProgramCommand(0x2102, name)`
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 待解析请求/响应体格式
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 同上
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 同上
|
||||
- `GetTcpAsync()` / `SetTcpAsync()` — 已按 `tcp_id + f32[7] pose` 字段布局实现
|
||||
- `GetSpeedRatioAsync()` / `SetSpeedRatioAsync()` — 已按 `ratio_int / 100.0` 与 `ratio_int_0_100` 字段布局实现
|
||||
- `GetIoAsync()` / `SetIoAsync()` — 已按 `io_type / io_index / f32 io_value` 字段布局实现
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucCommandClientTests.cs`
|
||||
- 用 `TcpListener` 本地模拟控制器，验证帧收发与解析
|
||||
@@ -80,11 +81,17 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改
|
||||
- 内部启动后台 `Task` 循环 `ReadAsync(FanucStateProtocol.StateFrameLength)`
|
||||
- 每收到一帧调用 `FanucStateProtocol.ParseFrame()`
|
||||
- 将解析结果写入线程安全的最新状态缓存
|
||||
- `GetLatestSnapshot()` — 返回最近一次解析的状态帧
|
||||
- 单帧接收超时后标记状态陈旧，不再把旧帧当作当前位姿/关节状态使用
|
||||
- EOF、坏帧、Socket 异常或超时后关闭当前连接，并按退避策略自动重连 TCP 10010
|
||||
- `GetLatestFrame()` — 返回最近一次解析的状态帧
|
||||
- `GetStatus()` — 返回连接阶段、陈旧状态、最近异常和重连次数
|
||||
- `Disconnect()` — 取消后台循环并关闭连接
|
||||
|
||||
**测试**：`tests/Flyshot.Core.Tests/FanucStateClientTests.cs`
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析
|
||||
- 用 `TcpListener` 本地发送抓包样本 hex，验证后台循环能正确解析。
|
||||
- 用本地模拟控制器验证无状态帧超时、EOF 后退避重连和重连后的继续收帧。
|
||||
- `FanucStateProtocol` 已用 `j519 协议.pcap` 中多条 90B 样本锁定 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]`。
|
||||
- 尾部状态字当前只作为 `ControllerStateSnapshot.stateTailWords` 诊断字段保留，不从 `[2,0,0,1]` 推断使能或运动状态。
|
||||
|
||||
### Phase 3: UDP 60015 J519 运动客户端
|
||||
|
||||
@@ -138,7 +145,7 @@ dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTest
|
||||
|
||||
1. **真机连接风险**：第一版 Socket 实现可能有超时/重连问题。`FanucControllerRuntime` 保留 `_simulationMode` 路径，仿真模式下仍走内存桩。
|
||||
2. **性能风险**：同步接口内部阻塞 Socket 可能影响 HTTP 并发。若实测有问题，后续将 `IControllerRuntime` 改为 async。
|
||||
3. **协议字段不完整风险**：TCP 10012 的 `GetTcp`/`SetTcp`/`GetIo`/`SetIo` 请求/响应体格式尚未完全逆向。先实现已知字段，留 TODO 标记待验证。
|
||||
3. **现场验证风险**：TCP 10012 参数命令已按逆向结论实现，但仍需在真实 R30iB 控制柜上确认默认 `tcp_id=1`、IO 类型/地址和错误码语义。
|
||||
|
||||
## 关键文件清单
|
||||
|
||||
|
||||
@@ -171,7 +171,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run tests to verify they pass**
|
||||
- [x] **Step 4: Run tests to verify they pass**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientTrajectoryOrchestrator -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
@@ -209,7 +209,7 @@ public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPoi
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run test to verify it fails**
|
||||
- [x] **Step 2: Run test to verify it fails**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: FAIL because current service still treats ordinary execution as "move to last waypoint".
|
||||
@@ -239,12 +239,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run focused tests to verify green**
|
||||
- [x] **Step 4: Run focused tests to verify green**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj --filter "ControllerClientCompatService|ControllerClientTrajectoryOrchestrator|FanucControllerRuntime" -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS.
|
||||
|
||||
- [ ] **Step 5: Run integration verification**
|
||||
- [x] **Step 5: Run integration verification**
|
||||
|
||||
Run: `dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal -m:1 -nodeReuse:false`
|
||||
Expected: PASS, with existing HTTP compatibility tests still green.
|
||||
|
||||
@@ -18,7 +18,8 @@ public sealed class ControllerStateSnapshot
|
||||
double speedRatio,
|
||||
IEnumerable<double>? jointPositions = null,
|
||||
IEnumerable<double>? cartesianPose = null,
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null)
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null,
|
||||
IEnumerable<uint>? stateTailWords = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -34,6 +35,7 @@ public sealed class ControllerStateSnapshot
|
||||
var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty<RuntimeAlarm>();
|
||||
var copiedStateTailWords = stateTailWords?.ToArray() ?? Array.Empty<uint>();
|
||||
|
||||
CapturedAt = capturedAt;
|
||||
ConnectionState = connectionState;
|
||||
@@ -43,6 +45,7 @@ public sealed class ControllerStateSnapshot
|
||||
JointPositions = copiedJointPositions;
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -92,4 +95,10 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("activeAlarms")]
|
||||
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 10010 状态帧尾部原始状态字，仅用于诊断，不直接推断运行语义。
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
}
|
||||
|
||||
@@ -24,6 +24,50 @@ public sealed class ICspPlanner
|
||||
/// </summary>
|
||||
public const int DefaultMaxIterations = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
|
||||
/// </summary>
|
||||
public const double DefaultFinalScaleTolerance = 1e-2;
|
||||
|
||||
private readonly double _threshold;
|
||||
private readonly int _maxIterations;
|
||||
private readonly bool _enforceFinalScale;
|
||||
private readonly double _finalScaleTolerance;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance)
|
||||
{
|
||||
if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
|
||||
}
|
||||
|
||||
if (maxIterations < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
|
||||
}
|
||||
|
||||
if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
|
||||
}
|
||||
|
||||
_threshold = threshold;
|
||||
_maxIterations = maxIterations;
|
||||
_enforceFinalScale = enforceFinalScale;
|
||||
_finalScaleTolerance = finalScaleTolerance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
@@ -52,7 +96,7 @@ public sealed class ICspPlanner
|
||||
int bestIterations = 0;
|
||||
double[]? bestWaypointTimes = null;
|
||||
|
||||
for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++)
|
||||
for (int iteration = 0; iteration <= _maxIterations; iteration++)
|
||||
{
|
||||
var waypointTimes = CumulativeTimes(segmentDurations);
|
||||
var spline = new CubicSplineInterpolator(waypointTimes, qs);
|
||||
@@ -89,7 +133,7 @@ public sealed class ICspPlanner
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < DefaultThreshold)
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
break;
|
||||
}
|
||||
@@ -105,6 +149,13 @@ public sealed class ICspPlanner
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
|
||||
}
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
|
||||
@@ -24,7 +24,7 @@ public sealed class SelfAdaptIcspPlanner
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner = new();
|
||||
private readonly ICspPlanner _innerPlanner = new(enforceFinalScale: false);
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
|
||||
|
||||
@@ -207,6 +207,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult();
|
||||
_speedRatio = response.Ratio;
|
||||
}
|
||||
|
||||
return _speedRatio;
|
||||
}
|
||||
}
|
||||
@@ -222,7 +228,13 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
_speedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
||||
var clampedRatio = Math.Clamp(ratio, 0.0, 1.0);
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_speedRatio = clampedRatio;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -232,6 +244,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp))
|
||||
{
|
||||
var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult();
|
||||
_tcp = response.Pose.Take(3).ToArray();
|
||||
}
|
||||
|
||||
return _tcp.ToArray();
|
||||
}
|
||||
}
|
||||
@@ -242,6 +260,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureRobotSetup();
|
||||
if (_activeControllerIsSimulation is false)
|
||||
{
|
||||
EnsureConnected();
|
||||
_commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_tcp = [x, y, z];
|
||||
}
|
||||
}
|
||||
@@ -259,6 +283,13 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureConnected();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult();
|
||||
_ioValues[(normalizedIoType, port)] = response.Value;
|
||||
return response.Value;
|
||||
}
|
||||
|
||||
return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value;
|
||||
}
|
||||
}
|
||||
@@ -276,6 +307,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
lock (_stateLock)
|
||||
{
|
||||
EnsureEnabled();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
_commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult();
|
||||
}
|
||||
|
||||
_ioValues[(normalizedIoType, port)] = value;
|
||||
}
|
||||
}
|
||||
@@ -288,10 +324,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
EnsureRobotSetup();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = _stateClient.GetLatestFrame();
|
||||
if (frame?.JointOrExtensionValues.Count >= _jointPositions.Length)
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame?.JointDegrees.Count >= _jointPositions.Length)
|
||||
{
|
||||
return frame.JointOrExtensionValues.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
return frame.JointDegrees.Take(_jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -307,10 +343,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
EnsureRobotSetup();
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = _stateClient.GetLatestFrame();
|
||||
if (frame?.Pose.Count >= 6)
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame?.CartesianPose.Count >= 6)
|
||||
{
|
||||
return frame.Pose.Take(6).Select(v => (double)v).ToArray();
|
||||
return frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -326,21 +362,24 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
var jointPositions = _jointPositions;
|
||||
var cartesianPose = _pose;
|
||||
var isInMotion = _isInMotion;
|
||||
IReadOnlyList<uint> stateTailWords = Array.Empty<uint>();
|
||||
|
||||
if (!IsSimulationMode)
|
||||
{
|
||||
var frame = _stateClient.GetLatestFrame();
|
||||
var frame = GetFreshStateFrame();
|
||||
if (frame is not null)
|
||||
{
|
||||
if (frame.JointOrExtensionValues.Count >= jointPositions.Length)
|
||||
if (frame.JointDegrees.Count >= jointPositions.Length)
|
||||
{
|
||||
jointPositions = frame.JointOrExtensionValues.Take(jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
jointPositions = frame.JointDegrees.Take(jointPositions.Length).Select(v => (double)v).ToArray();
|
||||
}
|
||||
|
||||
if (frame.Pose.Count >= 6)
|
||||
if (frame.CartesianPose.Count >= 6)
|
||||
{
|
||||
cartesianPose = frame.Pose.Take(6).Select(v => (double)v).ToArray();
|
||||
cartesianPose = frame.CartesianPose.Take(6).Select(v => (double)v).ToArray();
|
||||
}
|
||||
|
||||
stateTailWords = frame.RawTailWords.ToArray();
|
||||
}
|
||||
|
||||
var j519Response = _j519Client.GetLatestResponse();
|
||||
@@ -358,7 +397,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
speedRatio: _speedRatio,
|
||||
jointPositions: jointPositions,
|
||||
cartesianPose: cartesianPose,
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||||
activeAlarms: Array.Empty<RuntimeAlarm>(),
|
||||
stateTailWords: stateTailWords);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -582,6 +622,14 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
return ioType.Trim().ToUpperInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP 层三维 TCP 请求扩展为 FANUC 命令通道需要的 7 维 Pose。
|
||||
/// </summary>
|
||||
private static double[] CreateTcpPose(double x, double y, double z)
|
||||
{
|
||||
return [x, y, z, 0.0, 0.0, 0.0, 1.0];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验轨迹规划结果可执行。
|
||||
/// </summary>
|
||||
@@ -603,7 +651,50 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
|
||||
return "NotConfigured";
|
||||
}
|
||||
|
||||
return string.IsNullOrWhiteSpace(_connectedRobotIp) ? "Disconnected" : "Connected";
|
||||
if (string.IsNullOrWhiteSpace(_connectedRobotIp))
|
||||
{
|
||||
return "Disconnected";
|
||||
}
|
||||
|
||||
return _activeControllerIsSimulation is false
|
||||
? ResolveRealConnectionState(_stateClient.GetStatus())
|
||||
: "Connected";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把真实 10010 状态通道健康度映射为上层快照连接状态。
|
||||
/// </summary>
|
||||
internal static string ResolveRealConnectionState(FanucStateClientStatus status)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(status);
|
||||
|
||||
return status.State switch
|
||||
{
|
||||
FanucStateConnectionState.Connected when status.IsFrameStale => "StateTimeout",
|
||||
FanucStateConnectionState.Connected => "Connected",
|
||||
FanucStateConnectionState.TimedOut => "StateTimeout",
|
||||
FanucStateConnectionState.Reconnecting => "Reconnecting",
|
||||
FanucStateConnectionState.Connecting => "Connecting",
|
||||
_ => "Disconnected",
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断 runtime 是否可以把某个状态通道帧作为当前机器人状态使用。
|
||||
/// </summary>
|
||||
internal static bool ShouldUseStateFrame(FanucStateClientStatus status)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(status);
|
||||
return status.State == FanucStateConnectionState.Connected && !status.IsFrameStale;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取未超时的状态帧；超时或重连期间不把旧状态作为当前机器人状态使用。
|
||||
/// </summary>
|
||||
private FanucStateFrame? GetFreshStateFrame()
|
||||
{
|
||||
var status = _stateClient.GetStatus();
|
||||
return ShouldUseStateFrame(status) ? _stateClient.GetLatestFrame() : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -146,6 +146,87 @@ public sealed class FanucCommandClient : IDisposable
|
||||
return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
@@ -212,6 +293,45 @@ public sealed class FanucCommandClient : IDisposable
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验速度倍率响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 TCP 位姿响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 IO 读取响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private static FanucIoResponse EnsureSuccess(FanucIoResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造包含 FANUC 命令上下文的失败异常。
|
||||
/// </summary>
|
||||
|
||||
@@ -59,6 +59,54 @@ public static class FanucCommandMessageIds
|
||||
public const uint SetIo = 0x2209;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||||
/// </summary>
|
||||
public static class FanucIoTypes
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 数字输入 DI。
|
||||
/// </summary>
|
||||
public const uint DigitalInput = 1;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 数字输出 DO。
|
||||
/// </summary>
|
||||
public const uint DigitalOutput = 2;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输入 RI。
|
||||
/// </summary>
|
||||
public const uint RobotInput = 8;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输出 RO。
|
||||
/// </summary>
|
||||
public const uint RobotOutput = 9;
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
|
||||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||||
public static uint FromName(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant() switch
|
||||
{
|
||||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||||
"RI" or "KIOTYPERI" => RobotInput,
|
||||
"RO" or "KIOTYPERO" => RobotOutput,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||
/// </summary>
|
||||
@@ -91,6 +139,140 @@ public sealed class FanucCommandResultResponse
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||||
/// </summary>
|
||||
public sealed class FanucSpeedRatioResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化速度倍率响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
RatioInt = ratioInt;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的整数百分比。
|
||||
/// </summary>
|
||||
public uint RatioInt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||||
/// </summary>
|
||||
public double Ratio => RatioInt / 100.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||||
/// </summary>
|
||||
public sealed class FanucTcpResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 TCP 位姿响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
TcpId = tcpId;
|
||||
Pose = pose.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的 TCP ID。
|
||||
/// </summary>
|
||||
public uint TcpId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 7 维 TCP 位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||||
/// </summary>
|
||||
public sealed class FanucIoResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 IO 读取响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
NumericValue = numericValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的原始数值。
|
||||
/// </summary>
|
||||
public double NumericValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取按布尔 IO 解释后的值。
|
||||
/// </summary>
|
||||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||
/// </summary>
|
||||
@@ -166,6 +348,109 @@ public static class FanucCommandProtocol
|
||||
return PackFrame(messageId, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取速度倍率命令。
|
||||
/// </summary>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetSpeedRatioCommand()
|
||||
{
|
||||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||||
{
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||||
}
|
||||
|
||||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 布尔值。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
@@ -185,6 +470,70 @@ public static class FanucCommandProtocol
|
||||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||||
}
|
||||
|
||||
// GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
|
||||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||||
{
|
||||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
var pose = new double[7];
|
||||
for (int i = 0; i < pose.Length; i++)
|
||||
{
|
||||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||||
}
|
||||
|
||||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) + sizeof(float))
|
||||
{
|
||||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
|
||||
@@ -2,23 +2,153 @@ using System.Net.Sockets;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的连接阶段。
|
||||
/// </summary>
|
||||
public enum FanucStateConnectionState
|
||||
{
|
||||
/// <summary>
|
||||
/// 状态通道未连接。
|
||||
/// </summary>
|
||||
Disconnected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在建立连接。
|
||||
/// </summary>
|
||||
Connecting,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道已连接并由后台循环接收状态帧。
|
||||
/// </summary>
|
||||
Connected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道在限定时间内没有收到完整状态帧。
|
||||
/// </summary>
|
||||
TimedOut,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在按退避策略重新连接。
|
||||
/// </summary>
|
||||
Reconnecting,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10010 状态通道的超时和重连参数。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置接收一帧完整 90B 状态帧允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan FrameTimeout { get; init; } = TimeSpan.FromMilliseconds(250);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置初始重连等待时间。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectInitialDelay { get; init; } = TimeSpan.FromMilliseconds(100);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置重连等待时间的上限。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置单次 TCP 建连允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的当前诊断状态。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientStatus
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化状态通道诊断状态。
|
||||
/// </summary>
|
||||
public FanucStateClientStatus(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
DateTimeOffset? lastFrameAt,
|
||||
long reconnectAttemptCount,
|
||||
string? lastErrorMessage)
|
||||
{
|
||||
State = state;
|
||||
IsFrameStale = isFrameStale;
|
||||
LastFrameAt = lastFrameAt;
|
||||
ReconnectAttemptCount = reconnectAttemptCount;
|
||||
LastErrorMessage = lastErrorMessage;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前连接阶段。
|
||||
/// </summary>
|
||||
public FanucStateConnectionState State { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近缓存状态帧是否已经超过状态帧超时窗口。
|
||||
/// </summary>
|
||||
public bool IsFrameStale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次成功解析状态帧的 UTC 时间。
|
||||
/// </summary>
|
||||
public DateTimeOffset? LastFrameAt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取后台循环发起重连的累计次数。
|
||||
/// </summary>
|
||||
public long ReconnectAttemptCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次状态通道异常的诊断文本。
|
||||
/// </summary>
|
||||
public string? LastErrorMessage { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10010 状态通道客户端，持续接收状态帧并缓存最新快照。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClient : IDisposable
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly FanucStateClientOptions _options;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private Task? _receiveTask;
|
||||
private FanucStateFrame? _latestFrame;
|
||||
private FanucStateConnectionState _connectionState = FanucStateConnectionState.Disconnected;
|
||||
private DateTimeOffset? _lastConnectedAt;
|
||||
private DateTimeOffset? _lastFrameAt;
|
||||
private long _reconnectAttemptCount;
|
||||
private string? _lastErrorMessage;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 使用默认状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
public FanucStateClient()
|
||||
: this(new FanucStateClientOptions())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(options);
|
||||
ValidateOptions(options);
|
||||
_options = options;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||
public bool IsConnected => GetStatus().State == FanucStateConnectionState.Connected;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
|
||||
@@ -35,17 +165,44 @@ public sealed class FanucStateClient : IDisposable
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_tcpClient is not null)
|
||||
if (_receiveTask 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);
|
||||
using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _receiveCts.Token);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Connecting;
|
||||
_latestFrame = null;
|
||||
_lastConnectedAt = null;
|
||||
_lastFrameAt = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, linkedCts.Token).ConfigureAwait(false);
|
||||
}
|
||||
catch
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
}
|
||||
|
||||
_receiveCts.Dispose();
|
||||
_receiveCts = null;
|
||||
throw;
|
||||
}
|
||||
|
||||
_receiveTask = Task.Run(
|
||||
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
|
||||
_receiveCts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -55,31 +212,7 @@ public sealed class FanucStateClient : IDisposable
|
||||
{
|
||||
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;
|
||||
}
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -96,6 +229,25 @@ public sealed class FanucStateClient : IDisposable
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前诊断状态。
|
||||
/// </summary>
|
||||
/// <returns>状态通道诊断快照。</returns>
|
||||
public FanucStateClientStatus GetStatus()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return new FanucStateClientStatus(
|
||||
_connectionState,
|
||||
IsFrameStaleLocked(DateTimeOffset.UtcNow),
|
||||
_lastFrameAt,
|
||||
_reconnectAttemptCount,
|
||||
_lastErrorMessage);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
@@ -107,7 +259,226 @@ public sealed class FanucStateClient : IDisposable
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台循环：持续接收状态帧；断线、超时或坏帧后进入退避重连。
|
||||
/// </summary>
|
||||
private async Task ReceiveAndReconnectLoopAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var reconnectDelay = _options.ReconnectInitialDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
|
||||
reconnectDelay = _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
catch (TimeoutException ex)
|
||||
{
|
||||
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
|
||||
}
|
||||
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
|
||||
{
|
||||
MarkReceiveFailure(FanucStateConnectionState.Reconnecting, ex.Message);
|
||||
}
|
||||
|
||||
CloseCurrentConnection();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
reconnectDelay = await ReconnectWithBackoffAsync(ip, port, reconnectDelay, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前连接中持续读取状态帧，直到连接异常或被取消。
|
||||
/// </summary>
|
||||
private async Task ReceiveCurrentConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
NetworkStream stream;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream ?? throw new IOException("状态通道未连接。");
|
||||
}
|
||||
|
||||
var buffer = new byte[FanucStateProtocol.StateFrameLength];
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await ReadExactAsync(stream, buffer, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(buffer);
|
||||
lock (_stateLock)
|
||||
{
|
||||
_latestFrame = frame;
|
||||
_lastFrameAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取固定长度字节，超过帧超时窗口则抛出超时异常。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.FrameTimeout);
|
||||
|
||||
var totalRead = 0;
|
||||
try
|
||||
{
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await stream.ReadAsync(
|
||||
buffer.AsMemory(totalRead, buffer.Length - totalRead),
|
||||
timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("状态通道已断开，读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw new TimeoutException("状态通道接收超时，未在限定时间内收到完整 90B 状态帧。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开 TCP 状态通道并更新连接状态。
|
||||
/// </summary>
|
||||
private async Task OpenConnectionAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var tcpClient = new TcpClient { NoDelay = true };
|
||||
try
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.ConnectTimeout);
|
||||
await tcpClient.ConnectAsync(ip, port, timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_tcpClient = tcpClient;
|
||||
_stream = tcpClient.GetStream();
|
||||
_lastConnectedAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
tcpClient.Dispose();
|
||||
throw new TimeoutException("状态通道建连超时。");
|
||||
}
|
||||
catch
|
||||
{
|
||||
tcpClient.Dispose();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按退避策略循环尝试重新连接，并返回下一次异常后的退避时间。
|
||||
/// </summary>
|
||||
private async Task<TimeSpan> ReconnectWithBackoffAsync(
|
||||
string ip,
|
||||
int port,
|
||||
TimeSpan reconnectDelay,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var nextDelay = reconnectDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
}
|
||||
|
||||
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_reconnectAttemptCount++;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
return _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex) when (ex is SocketException or IOException or TimeoutException)
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
_lastErrorMessage = ex.Message;
|
||||
}
|
||||
|
||||
nextDelay = IncreaseReconnectDelay(nextDelay);
|
||||
}
|
||||
}
|
||||
|
||||
return nextDelay;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭当前 TCP 连接，不清除最新状态帧，供重连路径保留诊断数据。
|
||||
/// </summary>
|
||||
private void CloseCurrentConnection()
|
||||
{
|
||||
NetworkStream? stream;
|
||||
TcpClient? tcpClient;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream;
|
||||
tcpClient = _tcpClient;
|
||||
_stream = null;
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
stream?.Dispose();
|
||||
tcpClient?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录接收异常并更新状态通道连接阶段。
|
||||
/// </summary>
|
||||
private void MarkReceiveFailure(FanucStateConnectionState state, string message)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = state;
|
||||
_lastErrorMessage = message;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭后台循环和 socket 资源。
|
||||
/// </summary>
|
||||
private void Shutdown(bool clearLatestFrame)
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
CloseCurrentConnection();
|
||||
|
||||
try
|
||||
{
|
||||
@@ -115,74 +486,76 @@ public sealed class FanucStateClient : IDisposable
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 忽略取消异常。
|
||||
// 后台循环可能因取消而抛出 OperationCanceledException，忽略即可。
|
||||
}
|
||||
|
||||
_receiveTask?.Dispose();
|
||||
_receiveTask = null;
|
||||
_receiveCts?.Dispose();
|
||||
_stream?.Dispose();
|
||||
_tcpClient?.Dispose();
|
||||
}
|
||||
_receiveCts = null;
|
||||
|
||||
/// <summary>
|
||||
/// 后台循环：持续从流中读取固定长度状态帧并更新缓存。
|
||||
/// </summary>
|
||||
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
lock (_stateLock)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var buffer = new byte[FanucStateProtocol.StateFrameLength];
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
_lastConnectedAt = null;
|
||||
_lastErrorMessage = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
if (clearLatestFrame)
|
||||
{
|
||||
await ReadExactAsync(buffer, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(buffer);
|
||||
lock (_stateLock)
|
||||
{
|
||||
_latestFrame = frame;
|
||||
}
|
||||
_latestFrame = null;
|
||||
_lastFrameAt = null;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消，无需处理。
|
||||
}
|
||||
catch (IOException)
|
||||
{
|
||||
// 连接断开，退出循环。
|
||||
}
|
||||
catch (InvalidDataException)
|
||||
{
|
||||
// 解析到异常帧，退出循环由上层重连。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取固定长度字节。
|
||||
/// 判断缓存帧是否已经不能代表当前控制柜状态。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(byte[] buffer, CancellationToken cancellationToken)
|
||||
private bool IsFrameStaleLocked(DateTimeOffset now)
|
||||
{
|
||||
if (_stream is null)
|
||||
if (_latestFrame is null)
|
||||
{
|
||||
throw new InvalidOperationException("状态通道未连接。");
|
||||
return _connectionState is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting
|
||||
|| _reconnectAttemptCount > 0
|
||||
|| (_lastConnectedAt.HasValue && now - _lastConnectedAt.Value > _options.FrameTimeout);
|
||||
}
|
||||
|
||||
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。");
|
||||
}
|
||||
return _lastFrameAt.HasValue && now - _lastFrameAt.Value > _options.FrameTimeout;
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
/// <summary>
|
||||
/// 计算下一轮重连等待时间。
|
||||
/// </summary>
|
||||
private TimeSpan IncreaseReconnectDelay(TimeSpan currentDelay)
|
||||
{
|
||||
var doubledMilliseconds = Math.Max(currentDelay.TotalMilliseconds * 2.0, _options.ReconnectInitialDelay.TotalMilliseconds);
|
||||
var cappedMilliseconds = Math.Min(doubledMilliseconds, _options.ReconnectMaxDelay.TotalMilliseconds);
|
||||
return TimeSpan.FromMilliseconds(cappedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态通道参数，避免后台循环使用无效时间窗口。
|
||||
/// </summary>
|
||||
private static void ValidateOptions(FanucStateClientOptions options)
|
||||
{
|
||||
ValidatePositive(options.FrameTimeout, nameof(options.FrameTimeout));
|
||||
ValidatePositive(options.ReconnectInitialDelay, nameof(options.ReconnectInitialDelay));
|
||||
ValidatePositive(options.ReconnectMaxDelay, nameof(options.ReconnectMaxDelay));
|
||||
ValidatePositive(options.ConnectTimeout, nameof(options.ConnectTimeout));
|
||||
|
||||
if (options.ReconnectMaxDelay < options.ReconnectInitialDelay)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(options), "最大重连等待时间不能小于初始重连等待时间。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验时间参数必须为正值。
|
||||
/// </summary>
|
||||
private static void ValidatePositive(TimeSpan value, string parameterName)
|
||||
{
|
||||
if (value <= TimeSpan.Zero)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(parameterName, "时间参数必须大于 0。");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,6 +9,8 @@ public sealed class FanucStateFrame
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _jointOrExtensionValues;
|
||||
private readonly double[] _jointDegrees;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly uint[] _tailWords;
|
||||
|
||||
/// <summary>
|
||||
@@ -28,6 +30,24 @@ public sealed class FanucStateFrame
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
|
||||
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
|
||||
|
||||
if (_pose.Length != 6)
|
||||
{
|
||||
throw new ArgumentException("状态帧位姿必须包含 6 个 float。", nameof(pose));
|
||||
}
|
||||
|
||||
if (_jointOrExtensionValues.Length != 9)
|
||||
{
|
||||
throw new ArgumentException("状态帧关节/扩展轴必须包含 9 个 float。", nameof(jointOrExtensionValues));
|
||||
}
|
||||
|
||||
if (_tailWords.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
||||
}
|
||||
|
||||
_jointDegrees = _jointOrExtensionValues.Take(6).ToArray();
|
||||
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -40,15 +60,55 @@ public sealed class FanucStateFrame
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿 X/Y/Z/W/P/R，单位来自 FANUC 状态服务器。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> CartesianPose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的关节或扩展轴状态。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||
|
||||
/// <summary>
|
||||
/// 获取前 6 个机器人关节角度，单位为度。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧尾部状态槽位。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> TailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取原始尾部状态字。当前抓包中恒为 [2,0,0,1]，语义暂不强行推断。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> RawTailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 0 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord0 => _tailWords[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 1 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord1 => _tailWords[1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 2 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord2 => _tailWords[2];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 3 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord3 => _tailWords[3];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -114,6 +114,27 @@ public sealed class DomainModelTests
|
||||
Assert.Empty(snapshot.JointPositions);
|
||||
Assert.Empty(snapshot.CartesianPose);
|
||||
Assert.Empty(snapshot.ActiveAlarms);
|
||||
Assert.Empty(snapshot.StateTailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证控制器快照会保留 TCP 10010 尾部状态字作为诊断字段。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerStateSnapshot_CopiesStateTailWordsForDiagnostics()
|
||||
{
|
||||
var snapshot = new ControllerStateSnapshot(
|
||||
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
|
||||
connectionState: "Connected",
|
||||
isEnabled: true,
|
||||
isInMotion: false,
|
||||
speedRatio: 1.0,
|
||||
stateTailWords: [2u, 0u, 0u, 1u]);
|
||||
|
||||
var json = JsonSerializer.Serialize(snapshot);
|
||||
|
||||
Assert.Equal([2u, 0u, 0u, 1u], snapshot.StateTailWords);
|
||||
Assert.Contains("\"stateTailWords\":[2,0,0,1]", json);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -130,6 +130,125 @@ public sealed class FanucCommandClientTests : IDisposable
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetSpeedRatio 发送空业务体命令，并按 ratio_int / 100.0 解析倍率。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetSpeedRatioAsync_SendsFrameAndParsesRatio()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetSpeedRatioCommand(),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetSpeedRatio, Convert.FromHexString("0000005a00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetSpeedRatioAsync(_cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal(0.9, response.Ratio, precision: 6);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetSpeedRatio 会把 double 倍率夹到 0..100 的整数百分比后下发。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetSpeedRatioAsync_SendsClampedPercentAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetSpeedRatioCommand(2.0),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetSpeedRatio, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetSpeedRatioAsync(2.0, _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetTcp 会发送 tcp_id 请求，并解析 result_code + tcp_id + 7 个 float 位姿。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetTcpAsync_SendsFrameAndParsesPose()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetTcpCommand(1),
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetTcp,
|
||||
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetTcpAsync(1, _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], response.Pose);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetTcp 会按 tcp_id + 7 个 float 位姿下发并解析结果码。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetTcpAsync_SendsFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetTcp, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetTcpAsync(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 GetIo 会按 io_type、io_index 顺序请求，并解析 float IO 值。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetIoAsync_SendsFrameAndParsesValue()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetIo, Convert.FromHexString("000000003f800000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.GetIoAsync(7, "DO", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
Assert.True(response.Value);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 SetIo 会按 io_type、io_index、float value 顺序下发并解析结果码。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task SetIoAsync_SendsFrameAndParsesSuccess()
|
||||
{
|
||||
using var client = new FanucCommandClient();
|
||||
var handlerTask = RunSingleResponseControllerAsync(
|
||||
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true),
|
||||
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetIo, Convert.FromHexString("00000000")),
|
||||
_cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
var response = await client.SetIoAsync(7, true, "DO", _cts.Token);
|
||||
|
||||
Assert.True(response.IsSuccess);
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证命令响应 result_code 非零时，客户端会抛出可诊断异常而不是让上层误判成功。
|
||||
/// </summary>
|
||||
|
||||
@@ -39,6 +39,64 @@ public sealed class FanucProtocolTests
|
||||
Assert.Equal(1u, statusResponse.ProgramStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 的速度倍率、TCP 和 IO 请求体字段顺序与逆向文档一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_PacksParameterCommandBodies()
|
||||
{
|
||||
var setTcpFrame = FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
|
||||
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000000e000022067a6f64"),
|
||||
FanucCommandProtocol.PackGetSpeedRatioCommand());
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002207000000507a6f64"),
|
||||
FanucCommandProtocol.PackSetSpeedRatioCommand(0.8));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002200000000017a6f64"),
|
||||
FanucCommandProtocol.PackGetTcpCommand(1));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a000000160000220800000002000000077a6f64"),
|
||||
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001a0000220900000002000000073f8000007a6f64"),
|
||||
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true));
|
||||
Assert.Equal(FanucCommandMessageIds.SetTcp, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(7, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(11, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(15, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(39, 4)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 参数响应解析使用各自不同的字段顺序。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_ParsesParameterResponses()
|
||||
{
|
||||
var speedRatioResponse = FanucCommandProtocol.ParseSpeedRatioResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetSpeedRatio,
|
||||
Convert.FromHexString("0000005000000000")));
|
||||
var tcpResponse = FanucCommandProtocol.ParseTcpResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetTcp,
|
||||
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")));
|
||||
var ioResponse = FanucCommandProtocol.ParseIoResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetIo,
|
||||
Convert.FromHexString("000000003f800000")));
|
||||
|
||||
Assert.True(speedRatioResponse.IsSuccess);
|
||||
Assert.Equal(0.8, speedRatioResponse.Ratio, precision: 6);
|
||||
Assert.True(tcpResponse.IsSuccess);
|
||||
Assert.Equal(1u, tcpResponse.TcpId);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], tcpResponse.Pose);
|
||||
Assert.True(ioResponse.IsSuccess);
|
||||
Assert.True(ioResponse.Value);
|
||||
Assert.Equal(1.0, ioResponse.NumericValue, precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
|
||||
/// </summary>
|
||||
@@ -52,6 +110,52 @@ public sealed class FanucProtocolTests
|
||||
Assert.Equal(6, frame.Pose.Count);
|
||||
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
||||
Assert.Equal(frame.Pose, frame.CartesianPose);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
|
||||
Assert.Equal(frame.TailWords, frame.RawTailWords);
|
||||
Assert.Equal(2u, frame.StatusWord0);
|
||||
Assert.Equal(0u, frame.StatusWord1);
|
||||
Assert.Equal(0u, frame.StatusWord2);
|
||||
Assert.Equal(1u, frame.StatusWord3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 pcap 中多条唯一 TCP 10010 状态帧都符合固定 90B 布局。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData("646f7a0000005a0000000040eac85a43b2ef4043aba8e9421ed9c1c2828105c2ed981f3fbdbda0bed4764ebe92aacc3efd9f0a3f317ce9be5d4580000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a00000000415aab64440a5302439adef542b39739c293c441431d50423fcdb7003d862fe3beca5730bf60eab23f148e403f89269d000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a000000004221b6f9440b9ce043a129ac42b292bac29cba78431bddcb3fc743213d90268dbeba5351bf64bc1b3f0cbdf73f826864000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
public void StateProtocol_ParsesMultipleCapturedPcapFrames(string frameHex)
|
||||
{
|
||||
var frameBytes = Convert.FromHexString(frameHex);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(frameBytes);
|
||||
|
||||
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
|
||||
Assert.Equal(6, frame.CartesianPose.Count);
|
||||
Assert.Equal(6, frame.JointDegrees.Count);
|
||||
Assert.Equal(3, frame.ExternalAxes.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧会拒绝损坏的长度和 magic，避免后台循环缓存坏帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StateProtocol_RejectsMalformedStateFrames()
|
||||
{
|
||||
var validFrame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
var wrongMagic = validFrame.ToArray();
|
||||
wrongMagic[0] = 0;
|
||||
var wrongLength = validFrame.ToArray();
|
||||
wrongLength[6] = 0x59;
|
||||
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(validFrame.AsSpan(0, validFrame.Length - 1)));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongMagic));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongLength));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
@@ -96,8 +97,7 @@ public sealed class FanucStateClientTests : IDisposable
|
||||
public async Task Disconnect_ClearsLatestFrame()
|
||||
{
|
||||
using var client = new FanucStateClient();
|
||||
var capturedFrame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
var capturedFrame = CapturedStateFrame();
|
||||
|
||||
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
|
||||
|
||||
@@ -110,6 +110,69 @@ public sealed class FanucStateClientTests : IDisposable
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态通道长时间收不到完整帧时会标记陈旧并触发重连。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task GetStatus_MarksFrameStaleAndReconnectsWhenFrameTimesOut()
|
||||
{
|
||||
using var client = new FanucStateClient(new FanucStateClientOptions
|
||||
{
|
||||
FrameTimeout = TimeSpan.FromMilliseconds(100),
|
||||
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
|
||||
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
|
||||
ConnectTimeout = TimeSpan.FromSeconds(1),
|
||||
});
|
||||
|
||||
var acceptTask = _listener.AcceptTcpClientAsync(_cts.Token);
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
using var controller = await acceptTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
|
||||
await WaitUntilAsync(
|
||||
() => client.GetStatus().ReconnectAttemptCount > 0,
|
||||
TimeSpan.FromSeconds(2),
|
||||
_cts.Token);
|
||||
|
||||
var status = client.GetStatus();
|
||||
Assert.True(status.IsFrameStale);
|
||||
Assert.True(status.State is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting or FanucStateConnectionState.Connected);
|
||||
Assert.NotNull(status.LastErrorMessage);
|
||||
Assert.Contains("超时", status.LastErrorMessage);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证状态通道在控制柜主动断开后可以退避重连并接收新连接上的状态帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ReceiveLoop_ReconnectsAfterEofAndKeepsReceivingFrames()
|
||||
{
|
||||
using var client = new FanucStateClient(new FanucStateClientOptions
|
||||
{
|
||||
FrameTimeout = TimeSpan.FromMilliseconds(500),
|
||||
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
|
||||
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
|
||||
ConnectTimeout = TimeSpan.FromSeconds(1),
|
||||
});
|
||||
|
||||
var firstFrame = CapturedStateFrame(1);
|
||||
var secondFrame = CapturedStateFrame(2);
|
||||
var handlerTask = RunReconnectControllerAsync(firstFrame, secondFrame, _cts.Token);
|
||||
|
||||
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
|
||||
|
||||
await WaitUntilAsync(
|
||||
() => client.GetLatestFrame()?.MessageId == 2u,
|
||||
TimeSpan.FromSeconds(2),
|
||||
_cts.Token);
|
||||
|
||||
var status = client.GetStatus();
|
||||
Assert.Equal(FanucStateConnectionState.Connected, status.State);
|
||||
Assert.True(status.ReconnectAttemptCount >= 1);
|
||||
|
||||
client.Disconnect();
|
||||
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动模拟控制器，持续发送状态帧流。
|
||||
/// </summary>
|
||||
@@ -135,4 +198,66 @@ public sealed class FanucStateClientTests : IDisposable
|
||||
// 客户端断开。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动模拟控制器：第一条连接发一帧后主动断开，第二条连接持续发送新帧。
|
||||
/// </summary>
|
||||
private async Task RunReconnectControllerAsync(byte[] firstFrame, byte[] secondFrame, CancellationToken cancellationToken)
|
||||
{
|
||||
using (var firstController = await _listener.AcceptTcpClientAsync(cancellationToken))
|
||||
{
|
||||
await using var firstStream = firstController.GetStream();
|
||||
await firstStream.WriteAsync(firstFrame, cancellationToken);
|
||||
}
|
||||
|
||||
using var secondController = await _listener.AcceptTcpClientAsync(cancellationToken);
|
||||
await using var secondStream = secondController.GetStream();
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await secondStream.WriteAsync(secondFrame, cancellationToken);
|
||||
await Task.Delay(50, cancellationToken);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
// 正常取消。
|
||||
}
|
||||
catch (IOException)
|
||||
{
|
||||
// 客户端断开。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造来自 j519 抓包的状态帧，并按测试需要覆写 message_id。
|
||||
/// </summary>
|
||||
private static byte[] CapturedStateFrame(uint messageId = 0)
|
||||
{
|
||||
var frame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, 4), messageId);
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待异步后台循环达到预期状态，超时后让测试明确失败。
|
||||
/// </summary>
|
||||
private static async Task WaitUntilAsync(Func<bool> predicate, TimeSpan timeout, CancellationToken cancellationToken)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow + timeout;
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
if (predicate())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
await Task.Delay(20, cancellationToken);
|
||||
}
|
||||
|
||||
Assert.True(predicate(), "等待状态通道后台循环达到预期状态超时。");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -34,6 +34,30 @@ public sealed class PlanningCompatibilityTests
|
||||
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通 ICSP 在最终最优解仍超限时会显式失败，而不是返回不可执行轨迹。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
|
||||
{
|
||||
var request = new TrajectoryRequest(
|
||||
robot: CreateRobotProfile([0.1], [0.1], [0.1]),
|
||||
program: CreateProgram(
|
||||
new[]
|
||||
{
|
||||
new[] { 0.0 },
|
||||
new[] { 10.0 },
|
||||
new[] { 20.0 },
|
||||
new[] { 30.0 }
|
||||
}),
|
||||
method: PlanningMethod.Icsp);
|
||||
|
||||
var planner = new ICspPlanner(maxIterations: 0);
|
||||
|
||||
var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
|
||||
Assert.Contains("global_scale", exception.Message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
|
||||
/// </summary>
|
||||
|
||||
@@ -3,6 +3,7 @@ using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
|
||||
@@ -45,6 +46,58 @@ public sealed class RuntimeOrchestrationTests
|
||||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(FanucStateConnectionState.Connected, false, "Connected")]
|
||||
[InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
|
||||
[InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
|
||||
[InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
|
||||
[InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
|
||||
public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
string expected)
|
||||
{
|
||||
var status = new FanucStateClientStatus(
|
||||
state,
|
||||
isFrameStale,
|
||||
lastFrameAt: null,
|
||||
reconnectAttemptCount: 0,
|
||||
lastErrorMessage: null);
|
||||
|
||||
var actual = FanucControllerRuntime.ResolveRealConnectionState(status);
|
||||
|
||||
Assert.Equal(expected, actual);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(FanucStateConnectionState.Connected, false, true)]
|
||||
[InlineData(FanucStateConnectionState.Connected, true, false)]
|
||||
[InlineData(FanucStateConnectionState.Reconnecting, false, false)]
|
||||
[InlineData(FanucStateConnectionState.TimedOut, false, false)]
|
||||
[InlineData(FanucStateConnectionState.Disconnected, false, false)]
|
||||
public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
bool expected)
|
||||
{
|
||||
var status = new FanucStateClientStatus(
|
||||
state,
|
||||
isFrameStale,
|
||||
lastFrameAt: null,
|
||||
reconnectAttemptCount: 0,
|
||||
lastErrorMessage: null);
|
||||
|
||||
var actual = FanucControllerRuntime.ShouldUseStateFrame(status);
|
||||
|
||||
Assert.Equal(expected, actual);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user