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:
2026-04-27 00:18:50 +08:00
parent 390d066ece
commit 69fa3edd89
18 changed files with 1631 additions and 122 deletions

View File

@@ -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(), "等待状态通道后台循环达到预期状态超时。");
}
}